-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathmid_3d.py
127 lines (87 loc) · 3.05 KB
/
mid_3d.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
"""Coordinates of the midpoint and the quasi-midpoint of a non-parabolic segment."""
from math import inf, isclose, sqrt
from point import nPoint
from utility import PRECISION
def p(a: nPoint, b: nPoint, j: int, k: int) -> float:
"""Plucker coordinates. See above (3.1)."""
return a[j - 1] * b[k - 1] - a[k - 1] * b[j - 1]
def deltaI(a: nPoint, b: nPoint) -> float:
"""Formula (4.10)."""
return a[2] * b[3] + a[3] * b[2]
def deltaII(a: nPoint, b: nPoint) -> float:
"""Formula (4.24)."""
return a[0] * b[1] + a[1] * b[0]
def thetaI(a: nPoint, b: nPoint) -> float:
"""Formula (4.10)."""
d = deltaI(a, b)
p13 = p(a, b, 1, 3)
p14 = p(a, b, 1, 4)
p23 = p(a, b, 2, 3)
p24 = p(a, b, 2, 4)
p34 = p(a, b, 3, 4)
b1 = p13**2 + p23**2 - p34**2
b2 = p13 * p14 + p23 * p24
numerator = d * b1 - 2 * a[2] * b[2] * b2
b1 = p14**2 + p24**2 + p34**2
denominator = d * b1 - 2 * a[3] * b[3] * b2
# print(a, b)
# print(f'b1 = {b1}\nb2 = {b2}')
if isclose(abs(denominator), 0, rel_tol=PRECISION):
return inf
return -numerator / denominator
def thetaII(a: nPoint, b: nPoint) -> float:
"""Formula (4.24)."""
d = deltaII(a, b)
p12 = p(a, b, 1, 2)
p13 = p(a, b, 1, 3)
p23 = p(a, b, 2, 3)
b1 = d * (p12**2 + p13**2)
b2 = p13 * p23
numerator = b1 - 2 * a[0] * b[0] * b2
denominator = b1 - 2 * a[1] * b[1] * b2
if isclose(abs(denominator), 0, rel_tol=PRECISION):
return inf
return -numerator / denominator
def omegaI(a: nPoint, b: nPoint) -> float:
"""Formula (4.10)."""
numerator = a[2] * b[2] + thetaI(a, b) * a[3] * b[3]
d = deltaI(a, b)
if isclose(abs(d), 0, rel_tol=PRECISION):
return inf
return numerator / d
def omegaII(a: nPoint, b: nPoint) -> float:
"""Formula (4.24)."""
numerator = a[0] * b[0] + thetaII(a, b) * a[1] * b[1]
d = deltaII(a, b)
if isclose(abs(d), 0, rel_tol=PRECISION):
return inf
return numerator / deltaII(a, b)
def get_coords(a: nPoint, b: nPoint, eps=1) -> nPoint:
"""Compute coordinates of «middle»."""
p14 = p(a, b, 1, 4)
p24 = p(a, b, 2, 4)
p34 = p(a, b, 3, 4)
s = [0 for i in range(4)]
if isclose(abs(p14), 0, rel_tol=PRECISION)\
and isclose(abs(p24), 0, rel_tol=PRECISION)\
and isclose(abs(p34), 0, rel_tol=PRECISION):
omeg = omegaII(a, b)
underroot = omeg**2 - thetaII(a, b)
if underroot < 0:
return nPoint(inf, inf, inf, inf)
root = sqrt(underroot)
p12 = p(a, b, 1, 2)
s[0] = p12 * omeg + eps * p12 * root
s[1] = p12
s[2] = p(a, b, 1, 3) - p(a, b, 2, 3) * (omeg + eps * root)
return nPoint(*s)
omeg = omegaI(a, b)
underroot = omeg**2 - thetaI(a, b)
if underroot < 0:
return nPoint(inf, inf, inf, inf)
root = sqrt(underroot)
s[0] = p14 * omeg + eps * p14 * root - p(a, b, 1, 3)
s[1] = p24 * omeg + eps * p24 * root - p(a, b, 2, 3)
s[2] = p34 * omeg + eps * p34 * root
s[3] = p34
return nPoint(*s)