refactor(B05): 능선-계곡 길찾기 그래프 뼈대 분리 (787→552줄)
- `B05_Profile_Engine_RidgeValley_Graph.py`(257줄) 신설 — 격자 조회(`_Grid`), 노드 수집, 구획 마스크, 정속경사 세그먼트 판정, 엣지·연결 후보, 교각 계산. - 원본에는 탐색(`_search_segment`)·fillet 선형·진입점 계산만 남김. - 엣지 후보 상수(MAX_EDGE_LEN_M 등)도 함께 이동 — 쓰는 쪽이 전부 새 모듈. - 외부 호출부 불변(`solve_ridge_valley_route` 경로 동일), pytest 359 passed.
This commit is contained in:
@@ -19,6 +19,15 @@ from B05_Profile.B05_Profile_Engine_Geometry import (
|
||||
point_to_polyline_dist_2d,
|
||||
resample_polyline_2d,
|
||||
)
|
||||
from B05_Profile.B05_Profile_Engine_RidgeValley_Graph import (
|
||||
_build_barrier_mask,
|
||||
_build_edges,
|
||||
_collect_nodes,
|
||||
_endpoint_connectors,
|
||||
_Grid,
|
||||
_segment_feasible,
|
||||
_turn_angle,
|
||||
)
|
||||
from B05_Profile.B05_Profile_Engine_Skeleton import load_or_build_skeleton
|
||||
from B05_Profile.B05_Profile_Engine_Solver import (
|
||||
_MODELS_SUBDIR,
|
||||
@@ -34,255 +43,11 @@ from config.config_system import (
|
||||
SKELETON_NODE_SPACING_M,
|
||||
)
|
||||
|
||||
# 엣지 후보 탐색 파라미터 (알고리즘 내부 상수)
|
||||
MAX_EDGE_LEN_M = 400.0
|
||||
MIN_EDGE_LEN_M = 20.0
|
||||
MAX_NEIGHBORS_PER_NODE = 16
|
||||
# 교각 페널티 (알고리즘 내부 상수) — 엣지 후보 상수는 _Graph 로 옮겼다(2026-09-02).
|
||||
TURN_PENALTY_W = 60.0
|
||||
MAX_TURN_DEG = 120.0
|
||||
|
||||
|
||||
class _Grid:
|
||||
"""비용면 격자에 대한 표고/유효성 조회 헬퍼."""
|
||||
|
||||
def __init__(self, x, y, z, valid, grid_res):
|
||||
self.x = np.asarray(x, dtype=np.float64)
|
||||
self.y = np.asarray(y, dtype=np.float64)
|
||||
self.z = np.asarray(z, dtype=np.float64)
|
||||
self.valid = np.asarray(valid, dtype=bool)
|
||||
self.res = float(grid_res)
|
||||
|
||||
def _idx(self, coords: np.ndarray, v: float) -> int:
|
||||
i = int(np.clip(np.searchsorted(coords, v), 0, len(coords) - 1))
|
||||
j = max(i - 1, 0)
|
||||
return j if abs(v - coords[j]) <= abs(coords[i] - v) else i
|
||||
|
||||
def rc(self, px: float, py: float) -> tuple[int, int]:
|
||||
return self._idx(self.y, py), self._idx(self.x, px)
|
||||
|
||||
def z_at(self, px: float, py: float) -> float:
|
||||
r, c = self.rc(px, py)
|
||||
return float(self.z[r, c])
|
||||
|
||||
def valid_at(self, px: float, py: float) -> bool:
|
||||
in_bounds = (self.x[0] <= px <= self.x[-1]) and (self.y[0] <= py <= self.y[-1])
|
||||
if not in_bounds:
|
||||
return False
|
||||
r, c = self.rc(px, py)
|
||||
return bool(self.valid[r, c])
|
||||
|
||||
|
||||
def _collect_nodes(
|
||||
skeleton: dict[str, Any], spacing_m: float
|
||||
) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
|
||||
"""능선/계곡 polyline 정점을 spacing 간격으로 다운샘플해 노드 배열을 만든다."""
|
||||
pos, kind, on_main = [], [], []
|
||||
|
||||
def _add(polys, k, is_main):
|
||||
for item in polys:
|
||||
pl = item["polyline"]
|
||||
acc = spacing_m
|
||||
prev = None
|
||||
for p in pl:
|
||||
step = spacing_m if prev is None else math.hypot(p[0] - prev[0], p[1] - prev[1])
|
||||
acc += step
|
||||
prev = p
|
||||
if acc >= spacing_m:
|
||||
acc = 0.0
|
||||
pos.append([p[0], p[1], p[2]])
|
||||
kind.append(k)
|
||||
on_main.append(is_main)
|
||||
|
||||
_add(skeleton.get("minor_ridge", []), 0, False)
|
||||
_add(skeleton.get("minor_valley", []), 1, False)
|
||||
_add(skeleton.get("main_ridge", []), 0, True)
|
||||
_add(skeleton.get("main_valley", []), 1, True)
|
||||
|
||||
if not pos:
|
||||
return (np.zeros((0, 3)), np.zeros(0, dtype=np.int8), np.zeros(0, dtype=bool))
|
||||
return (
|
||||
np.asarray(pos, dtype=np.float64),
|
||||
np.asarray(kind, dtype=np.int8),
|
||||
np.asarray(on_main, dtype=bool),
|
||||
)
|
||||
|
||||
|
||||
def _build_barrier_mask(grid: _Grid, skeleton: dict[str, Any]) -> np.ndarray:
|
||||
"""주능선/주계곡 셀을 True로 표시한 구획 경계 마스크."""
|
||||
barrier = np.zeros(grid.z.shape, dtype=bool)
|
||||
for key in ("main_ridge", "main_valley"):
|
||||
for item in skeleton.get(key, []):
|
||||
for p in item["polyline"]:
|
||||
r, c = grid.rc(p[0], p[1])
|
||||
barrier[r, c] = True
|
||||
return barrier
|
||||
|
||||
|
||||
def _segment_feasible(
|
||||
a: np.ndarray,
|
||||
b: np.ndarray,
|
||||
grid: _Grid,
|
||||
barrier: np.ndarray,
|
||||
blocked_circles: list[dict[str, float]],
|
||||
min_grade: float,
|
||||
max_grade: float,
|
||||
tol: float,
|
||||
endpoint_free_m: float,
|
||||
enforce_grade_window: bool = True,
|
||||
) -> bool:
|
||||
"""a→b 직선이 정속경사 세그먼트로 성립하는지 검사한다."""
|
||||
dx, dy = b[0] - a[0], b[1] - a[1]
|
||||
length = math.hypot(dx, dy)
|
||||
if length < 1e-6:
|
||||
return False
|
||||
design_grade = (b[2] - a[2]) / length
|
||||
g = abs(design_grade)
|
||||
if enforce_grade_window:
|
||||
if not (min_grade <= g <= max_grade):
|
||||
return False
|
||||
elif g > max_grade:
|
||||
return False
|
||||
|
||||
step = max(grid.res, 1.0)
|
||||
n_steps = max(int(length / step), 1)
|
||||
for i in range(n_steps + 1):
|
||||
t = i / n_steps
|
||||
px, py = a[0] + t * dx, a[1] + t * dy
|
||||
if not grid.valid_at(px, py):
|
||||
return False
|
||||
s = t * length
|
||||
z_design = a[2] + design_grade * s
|
||||
z_terrain = grid.z_at(px, py)
|
||||
allowed = tol * max(s, length - s) + grid.res
|
||||
if abs(z_terrain - z_design) > allowed:
|
||||
return False
|
||||
if min(s, length - s) > endpoint_free_m:
|
||||
r, c = grid.rc(px, py)
|
||||
if barrier[r, c]:
|
||||
return False
|
||||
for circ in blocked_circles:
|
||||
if math.hypot(px - circ["x"], py - circ["y"]) < circ["radius_m"]:
|
||||
return False
|
||||
return True
|
||||
|
||||
|
||||
def _build_edges(
|
||||
pos: np.ndarray,
|
||||
kind: np.ndarray,
|
||||
grid: _Grid,
|
||||
barrier: np.ndarray,
|
||||
blocked_circles: list[dict[str, float]],
|
||||
min_grade: float,
|
||||
max_grade: float,
|
||||
tol: float,
|
||||
endpoint_free_m: float,
|
||||
) -> dict[int, list[tuple[int, float]]]:
|
||||
"""능선↔계곡 노드 쌍의 정속경사 직선 엣지를 만든다 (무방향, 길이 저장)."""
|
||||
from scipy.spatial import cKDTree
|
||||
|
||||
adj: dict[int, list[tuple[int, float]]] = {i: [] for i in range(len(pos))}
|
||||
if len(pos) == 0:
|
||||
return adj
|
||||
|
||||
ridge_idx = np.nonzero(kind == 0)[0]
|
||||
valley_idx = np.nonzero(kind == 1)[0]
|
||||
if len(ridge_idx) == 0 or len(valley_idx) == 0:
|
||||
return adj
|
||||
|
||||
valley_tree = cKDTree(pos[valley_idx, :2])
|
||||
for ri in ridge_idx:
|
||||
cand = valley_tree.query_ball_point(pos[ri, :2], MAX_EDGE_LEN_M)
|
||||
cand = sorted(
|
||||
cand,
|
||||
key=lambda j: (
|
||||
(pos[ri, 0] - pos[valley_idx[j], 0]) ** 2
|
||||
+ (pos[ri, 1] - pos[valley_idx[j], 1]) ** 2
|
||||
),
|
||||
)
|
||||
added = 0
|
||||
for j in cand:
|
||||
vi = int(valley_idx[j])
|
||||
length = math.hypot(pos[ri, 0] - pos[vi, 0], pos[ri, 1] - pos[vi, 1])
|
||||
if length < MIN_EDGE_LEN_M:
|
||||
continue
|
||||
if pos[ri, 2] <= pos[vi, 2]:
|
||||
continue
|
||||
if not _segment_feasible(
|
||||
pos[ri],
|
||||
pos[vi],
|
||||
grid,
|
||||
barrier,
|
||||
blocked_circles,
|
||||
min_grade,
|
||||
max_grade,
|
||||
tol,
|
||||
endpoint_free_m,
|
||||
):
|
||||
continue
|
||||
adj[int(ri)].append((vi, length))
|
||||
adj[vi].append((int(ri), length))
|
||||
added += 1
|
||||
if added >= MAX_NEIGHBORS_PER_NODE:
|
||||
break
|
||||
return adj
|
||||
|
||||
|
||||
def _endpoint_connectors(
|
||||
pt: dict[str, float],
|
||||
pos: np.ndarray,
|
||||
grid: _Grid,
|
||||
barrier: np.ndarray,
|
||||
blocked_circles: list[dict[str, float]],
|
||||
max_uphill_grade: float,
|
||||
max_downhill_grade: float,
|
||||
tol: float,
|
||||
endpoint_free_m: float,
|
||||
) -> list[tuple[int, float]]:
|
||||
"""BP/CP/EP를 그래프 노드에 잇는 연결 세그먼트 후보."""
|
||||
from scipy.spatial import cKDTree
|
||||
|
||||
if len(pos) == 0:
|
||||
return []
|
||||
p = np.array([pt["x"], pt["y"], grid.z_at(pt["x"], pt["y"])])
|
||||
tree = cKDTree(pos[:, :2])
|
||||
cand = tree.query_ball_point(p[:2], MAX_EDGE_LEN_M)
|
||||
cand = sorted(cand, key=lambda j: (p[0] - pos[j, 0]) ** 2 + (p[1] - pos[j, 1]) ** 2)
|
||||
out = []
|
||||
for j in cand:
|
||||
length = math.hypot(p[0] - pos[j, 0], p[1] - pos[j, 1])
|
||||
if length < 1e-6:
|
||||
out.append((int(j), max(length, 0.01)))
|
||||
continue
|
||||
applicable = max_uphill_grade if pos[j, 2] > p[2] else max_downhill_grade
|
||||
if _segment_feasible(
|
||||
p,
|
||||
pos[j],
|
||||
grid,
|
||||
barrier,
|
||||
blocked_circles,
|
||||
0.0,
|
||||
applicable,
|
||||
tol,
|
||||
endpoint_free_m,
|
||||
enforce_grade_window=False,
|
||||
):
|
||||
out.append((int(j), length))
|
||||
if len(out) >= MAX_NEIGHBORS_PER_NODE:
|
||||
break
|
||||
return out
|
||||
|
||||
|
||||
def _turn_angle(p_prev, p_curr, p_next) -> float:
|
||||
"""진행방향 변화(교각) [rad]. 0 = 직진."""
|
||||
v1 = (p_curr[0] - p_prev[0], p_curr[1] - p_prev[1])
|
||||
v2 = (p_next[0] - p_curr[0], p_next[1] - p_curr[1])
|
||||
n1, n2 = math.hypot(*v1), math.hypot(*v2)
|
||||
if n1 < 1e-9 or n2 < 1e-9:
|
||||
return 0.0
|
||||
cosang = max(-1.0, min(1.0, (v1[0] * v2[0] + v1[1] * v2[1]) / (n1 * n2)))
|
||||
return math.acos(cosang)
|
||||
|
||||
|
||||
def _search_segment(
|
||||
start_pt: dict[str, float],
|
||||
end_pt: dict[str, float],
|
||||
|
||||
@@ -0,0 +1,257 @@
|
||||
"""B05 능선-계곡 길찾기 — 그래프 뼈대(격자 조회·노드·엣지).
|
||||
|
||||
`B05_Profile_Engine_RidgeValley` 에서 떼어낸 앞단이다(700줄 제한, 2026-09-02).
|
||||
탐색·선형(fillet)·진입점 계산은 원래 파일에 남고, 여기에는 **비용면 격자 조회와
|
||||
정속경사 세그먼트 판정·엣지 생성**만 둔다. 호출부는 원래 파일뿐이다.
|
||||
"""
|
||||
|
||||
import math
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
# 엣지 후보 탐색 파라미터 (알고리즘 내부 상수)
|
||||
MAX_EDGE_LEN_M = 400.0
|
||||
MIN_EDGE_LEN_M = 20.0
|
||||
MAX_NEIGHBORS_PER_NODE = 16
|
||||
|
||||
|
||||
class _Grid:
|
||||
"""비용면 격자에 대한 표고/유효성 조회 헬퍼."""
|
||||
|
||||
def __init__(self, x, y, z, valid, grid_res):
|
||||
self.x = np.asarray(x, dtype=np.float64)
|
||||
self.y = np.asarray(y, dtype=np.float64)
|
||||
self.z = np.asarray(z, dtype=np.float64)
|
||||
self.valid = np.asarray(valid, dtype=bool)
|
||||
self.res = float(grid_res)
|
||||
|
||||
def _idx(self, coords: np.ndarray, v: float) -> int:
|
||||
i = int(np.clip(np.searchsorted(coords, v), 0, len(coords) - 1))
|
||||
j = max(i - 1, 0)
|
||||
return j if abs(v - coords[j]) <= abs(coords[i] - v) else i
|
||||
|
||||
def rc(self, px: float, py: float) -> tuple[int, int]:
|
||||
return self._idx(self.y, py), self._idx(self.x, px)
|
||||
|
||||
def z_at(self, px: float, py: float) -> float:
|
||||
r, c = self.rc(px, py)
|
||||
return float(self.z[r, c])
|
||||
|
||||
def valid_at(self, px: float, py: float) -> bool:
|
||||
in_bounds = (self.x[0] <= px <= self.x[-1]) and (self.y[0] <= py <= self.y[-1])
|
||||
if not in_bounds:
|
||||
return False
|
||||
r, c = self.rc(px, py)
|
||||
return bool(self.valid[r, c])
|
||||
|
||||
|
||||
def _collect_nodes(
|
||||
skeleton: dict[str, Any], spacing_m: float
|
||||
) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
|
||||
"""능선/계곡 polyline 정점을 spacing 간격으로 다운샘플해 노드 배열을 만든다."""
|
||||
pos, kind, on_main = [], [], []
|
||||
|
||||
def _add(polys, k, is_main):
|
||||
for item in polys:
|
||||
pl = item["polyline"]
|
||||
acc = spacing_m
|
||||
prev = None
|
||||
for p in pl:
|
||||
step = spacing_m if prev is None else math.hypot(p[0] - prev[0], p[1] - prev[1])
|
||||
acc += step
|
||||
prev = p
|
||||
if acc >= spacing_m:
|
||||
acc = 0.0
|
||||
pos.append([p[0], p[1], p[2]])
|
||||
kind.append(k)
|
||||
on_main.append(is_main)
|
||||
|
||||
_add(skeleton.get("minor_ridge", []), 0, False)
|
||||
_add(skeleton.get("minor_valley", []), 1, False)
|
||||
_add(skeleton.get("main_ridge", []), 0, True)
|
||||
_add(skeleton.get("main_valley", []), 1, True)
|
||||
|
||||
if not pos:
|
||||
return (np.zeros((0, 3)), np.zeros(0, dtype=np.int8), np.zeros(0, dtype=bool))
|
||||
return (
|
||||
np.asarray(pos, dtype=np.float64),
|
||||
np.asarray(kind, dtype=np.int8),
|
||||
np.asarray(on_main, dtype=bool),
|
||||
)
|
||||
|
||||
|
||||
def _build_barrier_mask(grid: _Grid, skeleton: dict[str, Any]) -> np.ndarray:
|
||||
"""주능선/주계곡 셀을 True로 표시한 구획 경계 마스크."""
|
||||
barrier = np.zeros(grid.z.shape, dtype=bool)
|
||||
for key in ("main_ridge", "main_valley"):
|
||||
for item in skeleton.get(key, []):
|
||||
for p in item["polyline"]:
|
||||
r, c = grid.rc(p[0], p[1])
|
||||
barrier[r, c] = True
|
||||
return barrier
|
||||
|
||||
|
||||
def _segment_feasible(
|
||||
a: np.ndarray,
|
||||
b: np.ndarray,
|
||||
grid: _Grid,
|
||||
barrier: np.ndarray,
|
||||
blocked_circles: list[dict[str, float]],
|
||||
min_grade: float,
|
||||
max_grade: float,
|
||||
tol: float,
|
||||
endpoint_free_m: float,
|
||||
enforce_grade_window: bool = True,
|
||||
) -> bool:
|
||||
"""a→b 직선이 정속경사 세그먼트로 성립하는지 검사한다."""
|
||||
dx, dy = b[0] - a[0], b[1] - a[1]
|
||||
length = math.hypot(dx, dy)
|
||||
if length < 1e-6:
|
||||
return False
|
||||
design_grade = (b[2] - a[2]) / length
|
||||
g = abs(design_grade)
|
||||
if enforce_grade_window:
|
||||
if not (min_grade <= g <= max_grade):
|
||||
return False
|
||||
elif g > max_grade:
|
||||
return False
|
||||
|
||||
step = max(grid.res, 1.0)
|
||||
n_steps = max(int(length / step), 1)
|
||||
for i in range(n_steps + 1):
|
||||
t = i / n_steps
|
||||
px, py = a[0] + t * dx, a[1] + t * dy
|
||||
if not grid.valid_at(px, py):
|
||||
return False
|
||||
s = t * length
|
||||
z_design = a[2] + design_grade * s
|
||||
z_terrain = grid.z_at(px, py)
|
||||
allowed = tol * max(s, length - s) + grid.res
|
||||
if abs(z_terrain - z_design) > allowed:
|
||||
return False
|
||||
if min(s, length - s) > endpoint_free_m:
|
||||
r, c = grid.rc(px, py)
|
||||
if barrier[r, c]:
|
||||
return False
|
||||
for circ in blocked_circles:
|
||||
if math.hypot(px - circ["x"], py - circ["y"]) < circ["radius_m"]:
|
||||
return False
|
||||
return True
|
||||
|
||||
|
||||
def _build_edges(
|
||||
pos: np.ndarray,
|
||||
kind: np.ndarray,
|
||||
grid: _Grid,
|
||||
barrier: np.ndarray,
|
||||
blocked_circles: list[dict[str, float]],
|
||||
min_grade: float,
|
||||
max_grade: float,
|
||||
tol: float,
|
||||
endpoint_free_m: float,
|
||||
) -> dict[int, list[tuple[int, float]]]:
|
||||
"""능선↔계곡 노드 쌍의 정속경사 직선 엣지를 만든다 (무방향, 길이 저장)."""
|
||||
from scipy.spatial import cKDTree
|
||||
|
||||
adj: dict[int, list[tuple[int, float]]] = {i: [] for i in range(len(pos))}
|
||||
if len(pos) == 0:
|
||||
return adj
|
||||
|
||||
ridge_idx = np.nonzero(kind == 0)[0]
|
||||
valley_idx = np.nonzero(kind == 1)[0]
|
||||
if len(ridge_idx) == 0 or len(valley_idx) == 0:
|
||||
return adj
|
||||
|
||||
valley_tree = cKDTree(pos[valley_idx, :2])
|
||||
for ri in ridge_idx:
|
||||
cand = valley_tree.query_ball_point(pos[ri, :2], MAX_EDGE_LEN_M)
|
||||
cand = sorted(
|
||||
cand,
|
||||
key=lambda j: (
|
||||
(pos[ri, 0] - pos[valley_idx[j], 0]) ** 2
|
||||
+ (pos[ri, 1] - pos[valley_idx[j], 1]) ** 2
|
||||
),
|
||||
)
|
||||
added = 0
|
||||
for j in cand:
|
||||
vi = int(valley_idx[j])
|
||||
length = math.hypot(pos[ri, 0] - pos[vi, 0], pos[ri, 1] - pos[vi, 1])
|
||||
if length < MIN_EDGE_LEN_M:
|
||||
continue
|
||||
if pos[ri, 2] <= pos[vi, 2]:
|
||||
continue
|
||||
if not _segment_feasible(
|
||||
pos[ri],
|
||||
pos[vi],
|
||||
grid,
|
||||
barrier,
|
||||
blocked_circles,
|
||||
min_grade,
|
||||
max_grade,
|
||||
tol,
|
||||
endpoint_free_m,
|
||||
):
|
||||
continue
|
||||
adj[int(ri)].append((vi, length))
|
||||
adj[vi].append((int(ri), length))
|
||||
added += 1
|
||||
if added >= MAX_NEIGHBORS_PER_NODE:
|
||||
break
|
||||
return adj
|
||||
|
||||
|
||||
def _endpoint_connectors(
|
||||
pt: dict[str, float],
|
||||
pos: np.ndarray,
|
||||
grid: _Grid,
|
||||
barrier: np.ndarray,
|
||||
blocked_circles: list[dict[str, float]],
|
||||
max_uphill_grade: float,
|
||||
max_downhill_grade: float,
|
||||
tol: float,
|
||||
endpoint_free_m: float,
|
||||
) -> list[tuple[int, float]]:
|
||||
"""BP/CP/EP를 그래프 노드에 잇는 연결 세그먼트 후보."""
|
||||
from scipy.spatial import cKDTree
|
||||
|
||||
if len(pos) == 0:
|
||||
return []
|
||||
p = np.array([pt["x"], pt["y"], grid.z_at(pt["x"], pt["y"])])
|
||||
tree = cKDTree(pos[:, :2])
|
||||
cand = tree.query_ball_point(p[:2], MAX_EDGE_LEN_M)
|
||||
cand = sorted(cand, key=lambda j: (p[0] - pos[j, 0]) ** 2 + (p[1] - pos[j, 1]) ** 2)
|
||||
out = []
|
||||
for j in cand:
|
||||
length = math.hypot(p[0] - pos[j, 0], p[1] - pos[j, 1])
|
||||
if length < 1e-6:
|
||||
out.append((int(j), max(length, 0.01)))
|
||||
continue
|
||||
applicable = max_uphill_grade if pos[j, 2] > p[2] else max_downhill_grade
|
||||
if _segment_feasible(
|
||||
p,
|
||||
pos[j],
|
||||
grid,
|
||||
barrier,
|
||||
blocked_circles,
|
||||
0.0,
|
||||
applicable,
|
||||
tol,
|
||||
endpoint_free_m,
|
||||
enforce_grade_window=False,
|
||||
):
|
||||
out.append((int(j), length))
|
||||
if len(out) >= MAX_NEIGHBORS_PER_NODE:
|
||||
break
|
||||
return out
|
||||
|
||||
|
||||
def _turn_angle(p_prev, p_curr, p_next) -> float:
|
||||
"""진행방향 변화(교각) [rad]. 0 = 직진."""
|
||||
v1 = (p_curr[0] - p_prev[0], p_curr[1] - p_prev[1])
|
||||
v2 = (p_next[0] - p_curr[0], p_next[1] - p_curr[1])
|
||||
n1, n2 = math.hypot(*v1), math.hypot(*v2)
|
||||
if n1 < 1e-9 or n2 < 1e-9:
|
||||
return 0.0
|
||||
cosang = max(-1.0, min(1.0, (v1[0] * v2[0] + v1[1] * v2[1]) / (n1 * n2)))
|
||||
return math.acos(cosang)
|
||||
Reference in New Issue
Block a user