-
Notifications
You must be signed in to change notification settings - Fork 2
/
utils.py
319 lines (262 loc) · 12.6 KB
/
utils.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
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
import numpy
from OCC.BRep import BRep_Builder, BRep_Tool_Pnt
from OCC.BRepBuilderAPI import BRepBuilderAPI_MakePolygon, BRepBuilderAPI_MakeWire, BRepBuilderAPI_Sewing, \
BRepBuilderAPI_Transform, BRepBuilderAPI_GTransform
from OCC.BRepIntCurveSurface import BRepIntCurveSurface_Inter
from OCC.BRepPrimAPI import BRepPrimAPI_MakePrism, BRepPrimAPI_MakeSphere
from OCC.SMESH import SMESH_Mesh
from OCC.TopAbs import TopAbs_EDGE, TopAbs_FACE, TopAbs_WIRE
from OCC.TopExp import TopExp_Explorer
from OCC.TopoDS import topods_Edge, topods_Face, topods_Wire, TopoDS_Compound, TopoDS_Shape
from OCC.gp import gp_Vec, gp_Pnt, gp_Dir, gp_Lin, gp_Trsf, gp_Quaternion, gp_Intrinsic_XYZ, gp_GTrsf
from OCCUtils import Topo
from OCCUtils.Construct import make_vertex, make_edge
def make_extrusion(face, length, vector=gp_Vec(0., 0., 1.)):
"""Creates an extrusion from a :param face, along the vector :param vector with a distance :param length.
By default, the extrusion is along the z axis.
:param face: A TopoDS_Face that should be extruded
:param length: The height of the extrusion
:param vector: A gp_Vec() Object. Gives the direction of the extrusion
:return: The extruded TopoDS_Shape
"""
vector.Normalize()
vector.Scale(length)
return BRepPrimAPI_MakePrism(face, vector).Shape()
def make_closed_polygon(*args):
mypoly = BRepBuilderAPI_MakePolygon()
for pt in args:
if isinstance(pt, list) or isinstance(pt, tuple):
for i in pt:
mypoly.Add(i)
else:
mypoly.Add(pt)
mypoly.Build()
mypoly.Close()
result = mypoly.Wire()
return result
def make_wire(*args):
# if we get an iterable, than add all edges to wire builder
if isinstance(args[0], list) or isinstance(args[0], tuple):
wire = BRepBuilderAPI_MakeWire()
for i in args[0]:
wire.Add(i)
# wire.Build()
return wire.Wire()
wire = BRepBuilderAPI_MakeWire(*args)
return wire.Wire()
def get_single_intersection_points(shape, global_x, global_y):
_list_gp_Pnt_intersection_points = []
_intersection = BRepIntCurveSurface_Inter()
_line = gp_Lin(gp_Pnt(global_x, global_y, 0), gp_Dir(0, 0, 1))
_intersection.Init(shape, _line, 0.0000001)
while _intersection.More():
_list_gp_Pnt_intersection_points.append(_intersection.Pnt())
print(_intersection.Pnt())
_intersection.Next()
return _list_gp_Pnt_intersection_points
def get_faces(shape):
""" return the faces from `shape`
:param shape: TopoDS_Shape, or a subclass like TopoDS_Solid
:return: a list of faces found in `shape`
"""
topexp = TopExp_Explorer()
topexp.Init(shape, TopAbs_FACE)
_faces = []
while topexp.More():
fc = topods_Face(topexp.Current())
_faces.append(fc)
topexp.Next()
return _faces
def get_wires(shape):
topexp = TopExp_Explorer()
topexp.Init(shape, TopAbs_WIRE)
_wires = []
while topexp.More():
fc = topods_Wire(topexp.Current())
_wires.append(fc)
topexp.Next()
return _wires
def get_edges(shape):
topexp = TopExp_Explorer()
topexp.Init(shape, TopAbs_EDGE)
_edges = []
while topexp.More():
fc = topods_Edge(topexp.Current())
_edges.append(fc)
topexp.Next()
return _edges
def draw_point_as_sphere(_gp_pnt, display_viewer3d, _radius=1):
if display_viewer3d is not None:
display_viewer3d.DisplayShape(BRepPrimAPI_MakeSphere(_gp_pnt, _radius).Shape()) # Origin
def draw_point_as_vertex(_gp_pnt, display_viewer3d):
if display_viewer3d is not None:
display_viewer3d.DisplayShape(make_vertex(_gp_pnt))
def draw_vector_as_edge(_gp_vec, _gp_pnt, display_viewer3d):
if display_viewer3d is not None:
display_viewer3d.DisplayShape(make_edge(gp_Lin(_gp_pnt, gp_Dir(_gp_vec))))
def draw_list_of_edges(_list_outer_outline, display_viewer3d):
if display_viewer3d is not None:
for _a_edge in _list_outer_outline:
display_viewer3d.DisplayShape(_a_edge)
def draw_list_of_faces(_list_faces, display_viewer3d):
if display_viewer3d is not None:
for _a_face in _list_faces:
display_viewer3d.DisplayShape(_a_face)
def multicore_update_ui_find_shadow_outline(my_update_queue, my_process_list):
processes_still_running = True
while processes_still_running:
print("\tProgress: ", end="")
_list_process_is_dead = []
for process in my_process_list:
if process.is_alive():
_list_process_is_dead.append(False)
else:
_list_process_is_dead.append(True)
if False in _list_process_is_dead:
processes_still_running = False
def list_to_compound_shape(new_list) -> TopoDS_Compound:
"""
Create a TopoDS_Compound from a list of e.g. faces
:param new_list: a list of TopoDS_Shape elements
:return: new_list as TopoDS_Compound object
"""
_compound = TopoDS_Compound()
a_builder = BRep_Builder()
a_builder.MakeCompound(_compound)
for _a_face in new_list:
a_builder.Add(_compound, _a_face)
return _compound
def sew_list(new_list):
_sewer = BRepBuilderAPI_Sewing()
first_run = True
for _a_face in new_list:
if first_run:
_sewer.Load(new_list[0])
first_run = False
else:
_sewer.Add(_a_face)
return _sewer.SewedShape()
def edge_vertices_to_numpy_array(_list_of_edges):
"""
Takes a list of TopoDS_Edges. Converts the vertices of all edges to a 2D numpy array. This array can be used by the
scipy.spatial.ConvexHull algo to create a convex hull from the vertices.
:param _list_of_edges: List of TopoDS_Edges
:return: A 2darray (numpy) with all koordinates of vertices
"""
_list_vertex_coords = [] # temp list to store all coords of the vertices
for _TopoEdge in _list_of_edges:
topo_explorer = Topo(_TopoEdge)
vertices_of_edge = topo_explorer.vertices()
for vertex in vertices_of_edge:
_list_vertex_coords.append([BRep_Tool_Pnt(vertex).X(), BRep_Tool_Pnt(vertex).Y()])
return numpy.array(_list_vertex_coords)
def find_unused_edges_in_convex_hull_old(_convex_hull, _list_all_edges):
"""
DEPRECATED: NOT USED!
See what vertices are used to create the convex hull. Remove corresponding edges from the list.
It can not be guaranteed that the returned list is 100% correct as convex hull may mix vertices form different edges
:param _convex_hull: The convex hull
:type _convex_hull: scipy.spatial.ConvexHull
:param _list_all_edges: A list of Edges that needs to be cleaned
:return: List of remaining edges that are not part of the convex hull
"""
i = 0
for _a_edge in _list_all_edges:
topo_exp = Topo(_a_edge)
vertices_of_edge = topo_exp.vertices()
for vertex in vertices_of_edge:
v_x = BRep_Tool_Pnt(vertex).X() # That little trick gets us the coords of a vertex
v_y = BRep_Tool_Pnt(vertex).Y()
for _vertex_index_in_hull in _convex_hull.vertices:
if v_x == _convex_hull.points[_vertex_index_in_hull][0] \
and v_y == _convex_hull.points[_vertex_index_in_hull][1]:
_list_all_edges.remove(_a_edge)
i += 1
# Problem: need to move on to next edge, python=pain_in_the_ass when you want to break nested loops
break
else:
continue # only executed if the inner loop did NOT break - Many thanks to stackoverflow.com
break # only executed if the inner loop DID break
else:
continue
print("Removed " + str(i) + " edges that are already part of convex hull from list")
return _list_all_edges
def get_shape_positon(new_shape: TopoDS_Shape) -> list:
"""
Returns the position of the shapes coordinate system relative to the origin of the scene (0,0,0)
When imported into the scene all geometry will sit at (0,0,0); (0,0,0)
:param new_shape: A TopoDS_Shape that you want to get the info from
:return: [translateX, translateY, translateZ, rotateX, rotateY, rotateZ] with rotation angles in rad
"""
loc = new_shape.Location() # get_shape_pos()
transfo = loc.Transformation()
trnasl_part = transfo.TranslationPart()
rotation = transfo.GetRotation()
x, y, z = rotation.GetEulerAngles(gp_Intrinsic_XYZ)
return [trnasl_part.X(), trnasl_part.Y(), trnasl_part.Z(), x, y, z]
def transform_shape(new_shape: TopoDS_Shape, trans_rot) -> TopoDS_Shape:
"""
Transform a TopoDS_Shape relative according to a vector and a rotation given by trans_rot list. The list can be obtained by
get_shape_positon(). It has the following structure [translateX, translateY, translateZ, rotateX, rotateY, rotateZ].
This function also accepts a homogeneous transformation matrix as parameter trans_rot
Use with caution. Might generate unexpected results when triangulation is accessed:
https://opencascade.blogspot.com/2009/02/continued.html#Triangulation
Use of BRepBuilderAPI_GTransform() should fix this but will not allow absolute transformations since the position
is overwritten
:param new_shape: The TopoDS_Shape you want to transform
:param trans_rot: [translateX, translateY, translateZ, rotateX, rotateY, rotateZ] in mm and rad OR a homogeneous
transformation matrix in form of a numpy.ndarray (4x4 or 3x3)
:return: The transformed TopoDS_Shape
"""
_translation = gp_Trsf()
if type(trans_rot) is list:
new_gp_quaternion = gp_Quaternion()
new_gp_quaternion.SetEulerAngles(gp_Intrinsic_XYZ, trans_rot[3], trans_rot[4],
trans_rot[5])
new_gp_vector_translation = gp_Vec(trans_rot[0], trans_rot[1], trans_rot[2])
_translation.SetTransformation(new_gp_quaternion, new_gp_vector_translation)
elif type(trans_rot) is numpy.ndarray:
# We got a homogeneous transformation matrix
_translation.SetValues(trans_rot[0][0], trans_rot[0][1], trans_rot[0][2], trans_rot[0][3],
trans_rot[1][0], trans_rot[1][1], trans_rot[1][2], trans_rot[1][3],
trans_rot[2][0], trans_rot[2][1], trans_rot[2][2], trans_rot[2][3])
# https://www.opencascade.com/content/changing-shape-transformation-and-performing-boolean-operations
_transformator = BRepBuilderAPI_GTransform(gp_GTrsf(_translation))
# This might cause problems as it does not transform underlying mesh from triangulation?
# _transformator = BRepBuilderAPI_Transform(_translation)
_transformator.Perform(new_shape)
return _transformator.Shape()
def apply_transformation(points_matrix: numpy.ndarray, homo_transfo_matrix: numpy.ndarray) -> numpy.ndarray:
"""
Does the same as cv2.ppf_match_3d.transformPCPose()
:param points_matrix: The points you want to transform in form of a matrix (3, 1) OR (x, 3) -> one point per row
:param homo_transfo_matrix: homogeneous transformation matrix
:return: the transformed points_matrix
"""
if points_matrix.shape == (3, 1):
# retruns (3, 1) numpy array of transformed 3D point
return numpy.matmul(homo_transfo_matrix[0:3, 0:3], points_matrix)
else:
# Allows to transform an entire array of points in one step
# (matrix with one point per row)
return numpy.matmul(homo_transfo_matrix[0:3, 0:3], points_matrix.T).T + homo_transfo_matrix[:3, 3].T
def set_axes_equal(ax):
def set_axes_radius(_ax, _origin, _radius):
_ax.set_xlim3d([_origin[0] - _radius, _origin[0] + _radius])
_ax.set_ylim3d([_origin[1] - _radius, _origin[1] + _radius])
_ax.set_zlim3d([_origin[2] - _radius, _origin[2] + _radius])
"""
Make axes of 3D plot have equal scale so that spheres appear as spheres,
cubes as cubes, etc.. This is one possible solution to Matplotlib's
ax.set_aspect('equal') and ax.axis('equal') not working for 3D.
:param _ax: a matplotlib axis, e.g., as output from plt.gca().
Source: https://stackoverflow.com/a/50664367
"""
limits = numpy.array([
ax.get_xlim3d(),
ax.get_ylim3d(),
ax.get_zlim3d(),
])
origin = numpy.mean(limits, axis=1)
radius = 0.5 * numpy.max(numpy.abs(limits[:, 1] - limits[:, 0]))
set_axes_radius(ax, origin, radius)