[planner] avoid rotation_matrix_2d since float inaccuracy breaks caching
This commit is contained in:
parent
22e645e527
commit
583bd5bd77
1 changed files with 9 additions and 5 deletions
|
|
@ -24,7 +24,7 @@ from __future__ import annotations
|
||||||
from collections.abc import Iterable, Mapping, Sequence
|
from collections.abc import Iterable, Mapping, Sequence
|
||||||
from dataclasses import dataclass
|
from dataclasses import dataclass
|
||||||
from itertools import combinations
|
from itertools import combinations
|
||||||
from math import isclose as math_isclose
|
from math import cos, isclose as math_isclose, sin
|
||||||
from typing import Any, Literal
|
from typing import Any, Literal
|
||||||
|
|
||||||
import numpy
|
import numpy
|
||||||
|
|
@ -33,7 +33,7 @@ from numpy.typing import ArrayLike
|
||||||
|
|
||||||
from ...error import BuildError, PortError
|
from ...error import BuildError, PortError
|
||||||
from ...ports import Port
|
from ...ports import Port
|
||||||
from ...utils import PTypeMatch, SupportsBool, ptype_match, ptypes_compatible, rotation_matrix_2d
|
from ...utils import PTypeMatch, SupportsBool, ptype_match, ptypes_compatible
|
||||||
from ..tools import (
|
from ..tools import (
|
||||||
BendOffer,
|
BendOffer,
|
||||||
PrimitiveKind,
|
PrimitiveKind,
|
||||||
|
|
@ -450,17 +450,21 @@ class Solver:
|
||||||
points back into the primitive, so each step advances orientation by
|
points back into the primitive, so each step advances orientation by
|
||||||
the primitive output rotation plus pi.
|
the primitive output rotation plus pi.
|
||||||
"""
|
"""
|
||||||
offset = numpy.zeros(2)
|
x = 0.0
|
||||||
|
y = 0.0
|
||||||
angle = 0.0
|
angle = 0.0
|
||||||
ptype: str | None = None
|
ptype: str | None = None
|
||||||
for step in steps:
|
for step in steps:
|
||||||
out_port = step.out_port
|
out_port = step.out_port
|
||||||
if out_port.rotation is None:
|
if out_port.rotation is None:
|
||||||
raise BuildError('Primitive endpoints must have rotation')
|
raise BuildError('Primitive endpoints must have rotation')
|
||||||
offset += rotation_matrix_2d(angle) @ out_port.offset
|
angle_cos = cos(angle)
|
||||||
|
angle_sin = sin(angle)
|
||||||
|
x += angle_cos * float(out_port.x) - angle_sin * float(out_port.y)
|
||||||
|
y += angle_sin * float(out_port.x) + angle_cos * float(out_port.y)
|
||||||
angle += out_port.rotation + pi
|
angle += out_port.rotation + pi
|
||||||
ptype = out_port.ptype
|
ptype = out_port.ptype
|
||||||
return Port(offset, rotation=angle - pi, ptype=ptype)
|
return Port((x, y), rotation=angle - pi, ptype=ptype)
|
||||||
|
|
||||||
def current_ptype(self, steps: Sequence[SelectedPrimitive]) -> str | None:
|
def current_ptype(self, steps: Sequence[SelectedPrimitive]) -> str | None:
|
||||||
return self.request.in_ptype if not steps else self.compose_endpoint(steps).ptype
|
return self.request.in_ptype if not steps else self.compose_endpoint(steps).ptype
|
||||||
|
|
|
||||||
Loading…
Add table
Add a link
Reference in a new issue