82 lines
2.1 KiB
Python
82 lines
2.1 KiB
Python
"""角度计算模块
|
||
|
||
角度定义（∠M2-O-M）：
|
||
- O: 圆心 / 固定旋转中心（第一帧选定，全程不变）
|
||
- M: 初始尾端参考位置（第一帧选定，全程不变）
|
||
- M2: 当前帧尾端位置（每帧重新选定）
|
||
- 角度 = 向量 O→M 与 O→M2 之间的夹角（0-180°）
|
||
"""
|
||
|
||
import math
|
||
from typing import Tuple
|
||
|
||
|
||
def angle_between_vectors(
|
||
v1: Tuple[float, float], v2: Tuple[float, float]
|
||
) -> float:
|
||
"""计算两个向量之间的夹角（度）"""
|
||
dot = v1[0] * v2[0] + v1[1] * v2[1]
|
||
mag1 = math.hypot(v1[0], v1[1])
|
||
mag2 = math.hypot(v2[0], v2[1])
|
||
if mag1 < 1e-9 or mag2 < 1e-9:
|
||
return 0.0
|
||
cos_theta = max(-1.0, min(1.0, dot / (mag1 * mag2)))
|
||
return math.degrees(math.acos(cos_theta))
|
||
|
||
|
||
def signed_angle(
|
||
o: Tuple[float, float],
|
||
m: Tuple[float, float],
|
||
m2: Tuple[float, float],
|
||
) -> float:
|
||
"""计算带符号的 ∠M2-O-M 角度
|
||
|
||
Args:
|
||
o: 圆心（顶点）
|
||
m: 初始尾端参考位置
|
||
m2: 当前尾端位置
|
||
|
||
Returns:
|
||
带符号的角度（度），正=顺时针，负=逆时针
|
||
"""
|
||
v_ref = (m[0] - o[0], m[1] - o[1]) # O → M
|
||
v_cur = (m2[0] - o[0], m2[1] - o[1]) # O → M2
|
||
|
||
angle = angle_between_vectors(v_ref, v_cur)
|
||
|
||
# 叉积 Z 分量判断方向
|
||
cross_z = v_ref[0] * v_cur[1] - v_ref[1] * v_cur[0]
|
||
if cross_z < 0:
|
||
angle = -angle
|
||
|
||
return angle
|
||
|
||
|
||
def curvature_from_three_points(
|
||
o: Tuple[float, float],
|
||
m: Tuple[float, float],
|
||
m2: Tuple[float, float],
|
||
scale: float = 1.0,
|
||
) -> float:
|
||
"""由三点计算曲率（Menger curvature）
|
||
|
||
Args:
|
||
o, m, m2: 三个点
|
||
scale: 像素到物理单位的比例（mm/pixel）
|
||
|
||
Returns:
|
||
曲率 (1/mm)
|
||
"""
|
||
a = math.hypot(o[0] - m[0], o[1] - m[1]) * scale
|
||
b = math.hypot(m[0] - m2[0], m[1] - m2[1]) * scale
|
||
c = math.hypot(m2[0] - o[0], m2[1] - o[1]) * scale
|
||
|
||
s = (a + b + c) / 2.0
|
||
area = math.sqrt(max(0, s * (s - a) * (s - b) * (s - c)))
|
||
|
||
if area < 1e-12:
|
||
return 0.0
|
||
|
||
radius = (a * b * c) / (4.0 * area)
|
||
return 1.0 / radius if radius > 1e-12 else 0.0
|