/
arc.py
205 lines (169 loc) · 6.34 KB
/
arc.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
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
import numpy as np
from .. import util
from ..constants import log
from ..constants import tol_path as tol
from ..constants import res_path as res
from .intersections import line_line
def arc_center(points):
"""
Given three points on an arc find:
center, radius, normal, and angle.
This uses the fact that the intersection of the perp
bisectors of the segments between the control points
is the center of the arc.
Parameters
---------
points : (3, dimension) float
Points in space, where dimension is either 2 or 3
Returns
---------
result : dict
Has keys:
'center': (d,) float, cartesian center of the arc
'radius': float, radius of the arc
'normal': (3,) float, the plane normal.
'angle': (2,) float, angle of start and end, in radians
'span' : float, angle swept by the arc, in radians
"""
# it's a lot easier to treat 2D as 3D with a zero Z value
points, is_2D = util.stack_3D(points, return_2D=True)
# find the two edge vectors of the triangle
edge_direction = np.diff(points, axis=0)
edge_midpoints = (edge_direction * 0.5) + points[:2]
# three points define a plane, so find signed normal
plane_normal = np.cross(*edge_direction[::-1])
plane_normal /= np.linalg.norm(plane_normal)
# unit vector along edges
vector_edge = util.unitize(edge_direction)
# perpendicular cector to each segment
vector_perp = util.unitize(np.cross(vector_edge, plane_normal))
# run the line-line intersection to find the point
intersects, center = line_line(origins=edge_midpoints,
directions=vector_perp,
plane_normal=plane_normal)
if not intersects:
raise ValueError('segments do not intersect:\n{}'.format(
str(points)))
# radius is euclidean distance
radius = ((points[0] - center) ** 2).sum() ** .5
# vectors from points on arc to center point
vector = util.unitize(points - center)
# find the angle between the first and last vector
angle = np.arccos(np.clip(np.dot(*vector[[0, 2]]), -1.0, 1.0))
# if the angle is nonzero and vectors are opposite directions
# it means we have a long arc rather than the short path
large_arc = (abs(angle) > tol.zero and
np.dot(*edge_direction) < 0.0)
if large_arc:
angle = (np.pi * 2) - angle
angles = np.arctan2(*vector[:, :2].T[::-1]) + np.pi * 2
angles_sorted = np.sort(angles[[0, 2]])
reverse = angles_sorted[0] < angles[1] < angles_sorted[1]
angles_sorted = angles_sorted[::(1 - int(not reverse) * 2)]
result = {'center': center[:(3 - is_2D)],
'radius': radius,
'normal': plane_normal,
'span': angle,
'angles': angles_sorted}
return result
def discretize_arc(points,
close=False,
scale=1.0):
"""
Returns a version of a three point arc consisting of
line segments.
Parameters
---------
points : (3, d) float
Points on the arc where d in [2,3]
close : boolean
If True close the arc into a circle
scale : float
What is the approximate overall drawing scale
Used to establish order of magnitude for precision
Returns
---------
discrete : (m, d) float
Connected points in space
"""
# make sure points are (n, 3)
points, is_2D = util.stack_3D(points, return_2D=True)
# find the center of the points
center_info = arc_center(points)
center, R, N, angle = (center_info['center'],
center_info['radius'],
center_info['normal'],
center_info['span'])
# if requested, close arc into a circle
if close:
angle = np.pi * 2
# the number of facets, based on the angle criteria
count_a = angle / res.seg_angle
count_l = ((R * angle)) / (res.seg_frac * scale)
# figure out the number of line segments
count = np.max([count_a, count_l])
# force at LEAST 4 points for the arc
# otherwise the endpoints will diverge
count = np.clip(count, 4, np.inf)
count = int(np.ceil(count))
V1 = util.unitize(points[0] - center)
V2 = util.unitize(np.cross(-N, V1))
t = np.linspace(0, angle, count)
discrete = np.tile(center, (count, 1))
discrete += R * np.cos(t).reshape((-1, 1)) * V1
discrete += R * np.sin(t).reshape((-1, 1)) * V2
# do an in-process check to make sure result endpoints
# match the endpoints of the source arc
if not close:
arc_dist = util.row_norm(points[[0, -1]] - discrete[[0, -1]])
arc_ok = (arc_dist < tol.merge).all()
if not arc_ok:
log.warning(
'failed to discretize arc (endpoint distance %s)',
str(arc_dist))
log.warning('Failed arc points: %s', str(points))
raise ValueError('Arc endpoints diverging!')
discrete = discrete[:, :(3 - is_2D)]
return discrete
def to_threepoint(center, radius, angles=None):
"""
For 2D arcs, given a center and radius convert them to three
points on the arc.
Parameters
-----------
center : (2,) float
Center point on the plane
radius : float
Radius of arc
angles : (2,) float
Angles in radians for start and end angle
if not specified, will default to (0.0, pi)
Returns
----------
three : (3, 2) float
Arc control points
"""
# if no angles provided assume we want a half circle
if angles is None:
angles = [0.0, np.pi]
# force angles to float64
angles = np.asanyarray(angles, dtype=np.float64)
if angles.shape != (2,):
raise ValueError('angles must be (2,)!')
# provide the wrap around
if angles[1] < angles[0]:
angles[1] += np.pi * 2
center = np.asanyarray(center, dtype=np.float64)
if center.shape != (2,):
raise ValueError('only valid on 2D arcs!')
# turn the angles of [start, end]
# into [start, middle, end]
angles = np.array([angles[0],
angles.mean(),
angles[1]],
dtype=np.float64)
# turn angles into (3,2) points
three = np.column_stack((np.cos(angles),
np.sin(angles))) * radius
three += center
return three