-
Notifications
You must be signed in to change notification settings - Fork 2
/
x3d_step_assy.py
1917 lines (1743 loc) · 91.3 KB
/
x3d_step_assy.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
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
from __future__ import division
from __future__ import print_function
'''
# Copyright (C) 2015, Elphel.inc.
# File: x3d_step_assy_color_match.py
# Generate x3d model from STEP parts models and STEP assembly
# by matching each solid in the assembly to the parts.
#
# Uses code from https://gist.github.com/hyOzd/2b38adff6a04e1613622
#
# This program is free software: you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
# GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License
# along with this program. If not, see <http:#www.gnu.org/licenses/>.
@author: Andrey Filippov
@copyright: 2015 Elphel, Inc.
@license: GPLv3.0+
@contact: [email protected]
@deffield updated: Updated
'''
from email import Errors
__author__ = "Andrey Filippov"
__copyright__ = "Copyright 2015, Elphel, Inc."
__license__ = "GPL"
__version__ = "3.0+"
__maintainer__ = "Andrey Filippov"
__email__ = "[email protected]"
__status__ = "Development"
import FreeCAD
import FreeCADGui # just to update console output (change to threads later?) - does not seem to work
import Part
import os
import time
import pickle
import math
import xml.etree.ElementTree as et
from xml.dom import minidom
from FreeCAD import Base
from PySide import QtCore, QtGui
from ConfigParser import SafeConfigParser
import sys
import traceback
CONFIG_PATH= "~/.FreeCAD/x3d_step_assy.ini"
ROOT_DIR = '~/parts/0393/export'
STEP_PARTS='~/parts/0393/export/step_parts'
#DIR_LIST = ["parts","subassy_flat"]
ASSEMBLY_PATH = ""
ASSEMBLY_SUFFIX = "-ASSY"
INFO_DIR = "info"
X3D_DIR = "x3d"
X3D_EXT = ".x3d"
INFO_EXT = ".pickle"
PRECISION = 0.0001
PRECISION_AREA = 0.001
PRECISION_VOLUME = 0.001
PRECISION_GYRATION = 0.001
PRECISION_INSIDE = 0.03
COLOR_PER_VERTEX = True
COMPONENTS = None # to hold data structure that is long to build so it will survive if the macro crashes
if CONFIG_PATH[0] == "~":
CONFIG_PATH = os.path.join(os.path.expanduser('~'),CONFIG_PATH[2:])
if ROOT_DIR[0] == "~":
ROOT_DIR = os.path.join(os.path.expanduser('~'),ROOT_DIR[2:])
if STEP_PARTS[0] == "~":
STEP_PARTS = os.path.join(os.path.expanduser('~'),STEP_PARTS[2:])
def get_step_list(dir_listdirs):
"""
@param dir_listdirs - a single directory path or a list of directories to scan for parts definitions as STEP files
@return a list of full paths of the STEP parts models
"""
if not isinstance(dir_listdirs,(list,tuple)):
dir_listdirs=[dir_listdirs]
return [os.path.join(root,f)
for dir_path in dir_listdirs if os.path.isdir(dir_path)
for root, _, files in os.walk(dir_path, topdown=True, onerror=None, followlinks = True)
for f in files if f.endswith((".step",".stp",".STP",".STEP"))]
def vector_to_tuple(v):
return((v.x,v.y,v.z))
def repair_solids_from_shells(shape):
"""
Some imported object from STEP files turned out to be open shells.
Convert them to solids with FreeCAD
@param shape - FreeCAD Shape
@return a list of FreeCAD solids
"""
solids = shape.Solids
new_solids = []
for sh in shape.Shells:
#find same shell in solids
for sld in solids:
if sh.isEqual(sld.Shells[0]):
new_solids.append(sld)
break
else:
new_solids.append(Part.Solid(sh))
return new_solids
#Find Vertex indices with maximal/minimal X,Y,Z to check orientation(Still does not check for holes - Add them somehow?
def verticesToCheck(solid):
"""
Create a list of 18 vertices having maximal/minimal X, Y, Z (and their +/- pairs).
These vertices will be later tested to be inside (with certain precision) the assembly part
@param solid - A Solid object
@return list of 18 3-tuples (x,y,z)
"""
l=[[],[],[],[],[],[],[],[],[]]
for v in solid.Vertexes:
l[0].append(v.X)
l[1].append(v.Y)
l[2].append(v.Z)
l[3].append(v.X + v.Y)
l[4].append(v.X - v.Y)
l[5].append(v.X + v.Z)
l[6].append(v.X - v.Z)
l[7].append(v.Y + v.Z)
l[8].append(v.Y - v.Z)
sind=set()
for lst in l:
sind.add(lst.index(min(lst)))
sind.add(lst.index(max(lst)))
lv=[]
for vi in sind:
v=solid.Vertexes[vi]
lv.append((v.X,v.Y,v.Z))
return lv
def getBoundBox(freecadObjects):
"""
Calculate BoundBox for all shapes in the document
@param freecadObjects - list of FreeCAD objects or all solids/shells
@return FreeCAD.BoundBox object for the whole document
"""
shells =[]
for o in freecadObjects:
if hasattr(o, "Shape"):
shape=o.Shape
for shell in shape.Shells: # solids and open shells
shells.append(shell)
elif hasattr(o, "BoundBox"):
shells.append(o)
bBox=None
for shell in shells:
thisBBox = shell.BoundBox
if not bBox:
bBox = FreeCAD.BoundBox(thisBBox.XMin,thisBBox.YMin,thisBBox.ZMin,
thisBBox.XMax,thisBBox.YMax,thisBBox.ZMax)
else:
bBox = FreeCAD.BoundBox(min(thisBBox.XMin,bBox.XMin),min(thisBBox.YMin,bBox.YMin),min(thisBBox.ZMin,bBox.ZMin),
max(thisBBox.XMax,bBox.XMax),max(thisBBox.YMax,bBox.YMax),max(thisBBox.ZMax,bBox.ZMax))
return bBox
def bBoxToX3d(bBox):
"""
Convert FreeCAD BoundBox to X3D representation (center, size)
@param bBox - FreeCAD BoundBox object
@return dictionary of {'center':(xc,yc,zc), size:(xs,ys,zs)}
"""
return {'center':((bBox.XMax + bBox.XMin)/2,(bBox.YMax + bBox.YMin)/2,(bBox.ZMax + bBox.ZMin)/2),
'size': ( bBox.XMax - bBox.XMin, bBox.YMax - bBox.YMin, bBox.ZMax - bBox.ZMin)}
#FreeCAD.BoundBox(0,0,0,0,0,0)
def create_file_info_nogui(shape, fname=""):
"""
A no-Gui version of the create_file_info, rather useless now as the color
is critical for the program. Using FreeCAD GGUI significantly slows down
the program and prevents it from running in a true batch mode. It seems
possible to hack Face.Tolerance property (unused so far) and import STEP
files saving colors in this property.
@param shape - FreeCAD Shape, containing one or more solids
@param fname - source file path
@return a pair of a list of info for each solid (as a dictionary) and a
list of solids in the shape
"""
objects = []
#repairing open shells
solids = shape.Solids
if len(solids) != len(shape.Shells):
print ("Repairing open shells that are not solids for %s"%(fname))
solids = repair_solids_from_shells(shape)
fromShell=True
else:
fromShell=False
for s in (solids):
pp=s.PrincipalProperties
objects.append({
"rpath": fname,
"shell": fromShell,
"volume": s.Volume,
"area": s.Area,
"center": vector_to_tuple(s.CenterOfMass),
"principal": {'RadiusOfGyration': pp['RadiusOfGyration'],
'FirstAxisOfInertia': vector_to_tuple(pp['FirstAxisOfInertia']),
'SecondAxisOfInertia': vector_to_tuple(pp['SecondAxisOfInertia']),
'ThirdAxisOfInertia': vector_to_tuple(pp['ThirdAxisOfInertia']),
'Moments': pp['Moments'],
'SymmetryPoint': pp['SymmetryPoint'],
'SymmetryAxis': pp['SymmetryAxis']},
"vertices": verticesToCheck(s)
})
# return objects
return (objects,solids)
def create_file_info(freecadObjects, fname=""):
"""
Collect information about each part/solid to be used for comparison between
assembly objects and parts
@param freecadObjects - list of FreeCAD objects
@param fname - source file path
@return a pair of a list of info for each solid (as a dictionary) and a
list of solids in the shape
"""
if not "Gui" in dir(FreeCAD):
return create_file_info_nogui(freecadObjects, fname)
# Count all shells in all objects
numShells = 0
for o in freecadObjects:
if hasattr(o, "Shape"):
numShells += len(o.Shape.Shells)
txt=""
if fname:
txt += " in "+fname
progress_bar = Base.ProgressIndicator()
progress_bar.start("Generating objects%s to export to X3D ..."%(txt), len(freecadObjects))
objects = []
allSolids=[]
for o in freecadObjects:
if hasattr(o, "Shape"):
shape=o.Shape
#repairing open shells
solids = shape.Solids
if len(solids) != len(shape.Shells):
print ("Repairing open shells that are not solids for %s"%(fname))
solids = repair_solids_from_shells(shape)
fromShell=True
else:
fromShell=False
# get all colors for faces in this object (normally just one Shell/Solid
color_set=set()
if o.ViewObject:
for clr in o.ViewObject.DiffuseColor: # colors are one per face
color_set.add(clr)
col_list = list(color_set)
col_dict={} # index for each color (reverse to list)
for i, clr in enumerate(col_list):
col_dict[clr] = i
#Calculate per-color centers for each object (normally each object has just one Solid/Shell
dc=o.ViewObject.DiffuseColor
if (len(dc) == 1) and (len(o.Shape.Faces)>1):
dc= dc * len(o.Shape.Faces)
colorCenters=[[0.0,0.0,0.0,0.0] for c in col_list] # SX,SY,SZ,S0
for clr,face in zip(dc, o.Shape.Faces):
clr_index = col_dict[clr]
m = face.Area
c = face.CenterOfMass # Vector
colorCenters[clr_index][0]+= c.x * m
colorCenters[clr_index][1]+= c.y * m
colorCenters[clr_index][2]+= c.z * m
colorCenters[clr_index][3]+= m
# print ("%s: cx=%f, cy=%f, cz=%f, m=%f"%(fname, c.x,c.y,c.z,m))
color_center_area={}
for clr in col_dict:
clr_index = col_dict[clr]
color_center_area[clr]={"center":(colorCenters[clr_index][0]/colorCenters[clr_index][3],
colorCenters[clr_index][1]/colorCenters[clr_index][3],
colorCenters[clr_index][2]/colorCenters[clr_index][3]),
"area": colorCenters[clr_index][3]}
# print ("color_center_area[%s] = %s"%(str(clr), str(color_center_area[clr])))
for i, s in enumerate(solids):
pp=s.PrincipalProperties
object={
"rpath": fname,
"shell": fromShell,
"volume": s.Volume,
"area": s.Area,
"center": vector_to_tuple(s.CenterOfMass),
"principal": {'RadiusOfGyration': pp['RadiusOfGyration'],
'FirstAxisOfInertia': vector_to_tuple(pp['FirstAxisOfInertia']),
'SecondAxisOfInertia': vector_to_tuple(pp['SecondAxisOfInertia']),
'ThirdAxisOfInertia': vector_to_tuple(pp['ThirdAxisOfInertia']),
'Moments': pp['Moments'],
'SymmetryPoint': pp['SymmetryPoint'],
'SymmetryAxis': pp['SymmetryAxis']},
"vertices": verticesToCheck(s)
}
if i == 0:
object["colorCenters"] = color_center_area
objects.append(object)
allSolids.append(s)
progress_bar.next() # True) # True - enable ESC to abort
progress_bar.stop()
return (objects,allSolids)
def get_info_files_nogui(dir_list = None):
"""
a no-gui version of get_info_files()
@param dir_list - list of directories (usually a single-element) to scan for
STEP part models (including subdirectories). Non-existing
directories in the list are OK, they will be silently skipped.
@return a dictionary with part names as keys and info parameters lists of
dictionaries created by create_file_info() as values
Each part usually has just one solid, but may have more than one, in that
case only the largest (by volume) is used for identification in the
assembly, and it is returned at index 0 in the result
"""
if dir_list is None:
dir_list = [STEP_PARTS]
start_time=time.time()
sl = get_step_list(dir_list = dir_list)
if not INFO_DIR in os.listdir(ROOT_DIR):
os.mkdir(os.path.join(ROOT_DIR,INFO_DIR))
todo_list = []
for f in sl: # now f is a full absolute path
fname,_ = os.path.splitext(os.path.basename(f))
info_path = os.path.join(ROOT_DIR,INFO_DIR,fname+INFO_EXT)
step_file = f # os.path.join(ROOT_DIR, f)
if (not os.path.isfile(info_path)) or (os.path.getmtime(step_file) > os.path.getmtime(info_path)): # no info or step is newer
todo_list.append(f)
for i, apath in enumerate(todo_list):
# apath = os.path.join(ROOT_DIR,f)
rslt_path = os.path.join(ROOT_DIR,INFO_DIR, os.path.splitext(os.path.basename(apath))[0] + INFO_EXT)
print("%d: Reading %s @%f"%(i,apath, time.time()-start_time), end="...")
shape = Part.read(apath)
print(" got %d solids @%f"%(len(shape.Solids), time.time()-start_time))
objects,_ = create_file_info_nogui(shape, apath)
print (objects)
pickle.dump(objects, open(rslt_path, "wb" ))
# Now read all pickled data:
info_dict = {}
for f in sl:
name = os.path.splitext(os.path.basename(f))[0]
info_path = os.path.join(ROOT_DIR,INFO_DIR, name + INFO_EXT)
info_dict[name] = pickle.load(open(info_path, "rb"))
#Put largest element as the [0] index
for k in info_dict:
if len(info_dict[k]) >1:
o = info_dict[k]
print (k,len(o),o)
vols = [s["volume"] for s in o]
mi = vols.index(max(vols))
print ("Largest solid is number %d"%(mi))
if mi>0:
o.insert(0,o.pop(mi))
return info_dict
def get_info_files(dir_list = None):
"""
Get information about each part collected with create_file_info() function
Generate this information for each part that does not have it or have
obsolete (older than STEP file) one.
@param dir_list - list of directories (usually a single-element) to scan for
STEP part models (including subdirectories). Non-existing
directories in the list are OK, they will be silently skipped.
@return a dictionary with part names as keys and info parameters lists of
dictionaries created by create_file_info() as values
Each part usually has just one solid, but may have more than one, in that
case only the largest (by volume) is used for identification in the
assembly, and it is returned at index 0 in the result
"""
if dir_list is None:
dir_list = [STEP_PARTS]
if not "Gui" in dir(FreeCAD):
return get_info_files_nogui(dir_list)
start_time=time.time()
sl = get_step_list(dir_listdirs = dir_list)
if not INFO_DIR in os.listdir(ROOT_DIR):
os.mkdir(os.path.join(ROOT_DIR,INFO_DIR))
todo_list = []
for f in sl: # now f is a full absolute path
print (f)
fname,_ = os.path.splitext(os.path.basename(f))
info_path = os.path.join(ROOT_DIR,INFO_DIR,fname+INFO_EXT)
step_file = f # os.path.join(ROOT_DIR, f)
if (not os.path.isfile(info_path)) or (os.path.getmtime(step_file) > os.path.getmtime(info_path)): # no info or step is newer
todo_list.append(f)
for i, apath in enumerate(todo_list):
# apath=os.path.join(ROOT_DIR,f)
rslt_path = os.path.join(ROOT_DIR,INFO_DIR, os.path.splitext(os.path.basename(apath))[0] + INFO_EXT)
print("%d: Reading %s @%f"%(i,apath, time.time()-start_time), end="...")
# Prepare data
FreeCAD.loadFile(apath)
doc = FreeCAD.activeDocument()
doc.Label = fname
print(" got %d objects @%f"%(len(doc.Objects), time.time()-start_time))
objects,_ = create_file_info(doc.Objects, apath)
FreeCAD.closeDocument(doc.Name)
FreeCADGui.updateGui()
# print (objects)
pickle.dump(objects, open(rslt_path, "wb" ))
# Now read all pickled data:
info_dict = {}
progress_bar = Base.ProgressIndicator()
progress_bar.start("Reading %d part info files ..."%(len(sl)), len(sl))
for f in sl:
name = os.path.splitext(os.path.basename(f))[0]
info_path = os.path.join(ROOT_DIR,INFO_DIR, name + INFO_EXT)
info_dict[name] = pickle.load(open(info_path, "rb"))
progress_bar.next()
progress_bar.stop()
# FreeCAD.Console.PrintMessage("get_info_files() - loaded"); #Rare FreeCAD crash?
#Put largest element of multi-solid parts as the [0] index.
for k in info_dict:
if len(info_dict[k]) >1:
o = info_dict[k]
print (k,len(o),o)
vols = [s["volume"] for s in o]
# vols = [s['principal']['RadiusOfGyration'][0] for s in o] # RadiusOfGyration[2] better characterizes the outer(larger) object?
# Maybe it is just outer thread?
mi = vols.index(max(vols))
print ("Largest solid is number %d"%(mi))
if mi>0:
o.insert(0,o.pop(mi))
# FreeCAD.Console.PrintMessage("get_info_files() - largest made first"); # rare FreeCAD crash?
return info_dict
def findPartsTransformations(solids, objects, candidates, info_dict, insidePrecision = PRECISION_INSIDE, precision = PRECISION):
"""
Find transformation (translation+rotation) matrices for each assembly part and each candidate part
@param solids - list of solids, they are used to check that the test part vertices are almost inside
Number of elements in solids, objects, candidates should match
@param objects - list of the solid properties (as dictionaries made by create_file_info()) of the assembly
elements.
@param candidates - list (per assembly element) of dictionaries indexed by part name (usually just one),
containing list of colors (tuples) for which there is an area match between assembly
element and a part. Build element part frame from colors first, then (if not enough)
use inertial directions.
@param info_dict - dictionary (part name as a key) of lists (first element is the largest by volume) of
part solid properties used for matching
@param inside_precision - precision for determining if the test points (available for each part) get inside
the assembly element. Currently as a fraction of the object bounding box diagonal,
but maybe it is better to use fraction of the translation distance plus diagonal?
@param precision - relative precision for matrix/vector calculations (determining co-linear/co-planar objects
@return a list (per assembly solid) of dictionaries part_name -> 4x4 transformation matrix (not yet tested
with multiple fits
"""
progress_bar = Base.ProgressIndicator()
progress_bar.start("Finding transformations for library parts to match assembly elements ...", len(objects))
transformations=[]
for i,s in enumerate(solids):
tolerance = insidePrecision * s.BoundBox.DiagonalLength # Or should it be fraction of the translation distance?
trans={}
print ("%d findPartsTransformations:"%(i))
for cand_name in candidates[i]:
co = info_dict[cand_name][0] # First solid in the candidate part file
try:
colorCenters = co['colorCenters']
except:
colorCenters = {}
matrix_part = ppToMatrix(co['principal'],co['center'], colorCenters, candidates[i][cand_name], 0, precision)
# Now try 4 orientations (until the first match).
# TODO - process parts with rotational axis (that allows certain, but not any rotation)
matrix_part_inverse = matrix_part.inverse()
# get color properties of a solid
try:
colorCenters = objects[i]['colorCenters']
except:
colorCenters = {}
for orient in range(4):
matrix_assy = ppToMatrix(s.PrincipalProperties,s.CenterOfMass,colorCenters, candidates[i][cand_name], orient, precision)
matrix_part_assy = matrix_assy.multiply(matrix_part_inverse)
for j, v in enumerate (co['vertices']):
if not s.isInside(matrix_part_assy.multiply(FreeCAD.Vector(v)),tolerance,True):
# print("%d: %s Failed on orientation %d vertice #%d (%f, %f,%f)"%(i,cand_name, orient, j, v[0],v[1],v[2]))
break
else:
print("%d: %s - got transformation with orientation %d"%(i,cand_name, orient))
# trans.append(matrix_part_assy)
trans[cand_name] = matrix_part_assy
break
else:
print("Could not find match for part %s, trying manually around that vertex"%(cand_name))
# Seems to be a bug FreeCAD does not recognize seemingly perfect match even with huge tolerance
# Will try manually around that point to find inside one
try_vectors= ((-1,-1,-1), (-1,-1, 0), (-1,-1, 1),
(-1, 0,-1), (-1, 0, 0), (-1, 0, 1),
(-1, 1,-1), (-1, 1, 0), (-1, 1, 1),
( 0,-1,-1), ( 0,-1, 0), ( 0,-1, 1),
( 0, 0,-1), ( 0, 0, 1),
( 0, 1,-1), ( 0, 1, 0), ( 0, 1, 1),
( 1,-1,-1), ( 1,-1, 0), ( 1,-1, 1),
( 1, 0,-1), ( 1, 0, 0), ( 1, 0, 1),
( 1, 1,-1), ( 1, 1, 0), ( 1, 1, 1))
for orient in range(4):
matrix_assy = ppToMatrix(s.PrincipalProperties,s.CenterOfMass,colorCenters, candidates[i][cand_name], orient, precision)
matrix_part_assy = matrix_assy.multiply(matrix_part_inverse)
for j, v in enumerate (co['vertices']):
if not s.isInside(matrix_part_assy.multiply(FreeCAD.Vector(v)),tolerance,True):
for tv in try_vectors:
mv= FreeCAD.Vector(v[0]+tv[0]*tolerance, v[1]+tv[1]*tolerance, v[2]+tv[2]*tolerance)
if s.isInside(matrix_part_assy.multiply(mv),tolerance,True):
break # got it!
else:
break # no luck
else:
print("%d: %s - finally got transformation with orientation %d"%(i,cand_name, orient))
# trans.append(matrix_part_assy)
trans[cand_name] = matrix_part_assy
break
else:
# trans.append(None) # so Transformations have same structure as candidates, and it is now dictionary
print("*** Could not find match for part %s"%(cand_name))
transformations.append(trans)
progress_bar.next() # True)
progress_bar.stop()
return transformations
def colorMatchCandidate(assy_object, candidates, info_dict, precision = PRECISION_AREA):
"""
Select colored features among the parts candidates by comparing total area per color
with the candidates so if some feature on the assembly object and the part have
different colors, the others can still be used for orientation identification.
Parts w/o matching color information can only be oriented by axes of gyration
@param assy_object - a dictionary of the parameters of the assembly object
@param candidates - a list of part name that fit this assembly object without color
properties
@param info_dict - dictionary of parameters for all parts (indexed by part names)
@return dictionary partName -> list of colors (as 3-tuples)
"""
colored_candidates={}
if candidates:
try:
assy_color_center_area = assy_object["colorCenters"]
except:
print ("colorMatchCandidate(), assy_object = ",assy_object)
assy_color_center_area = {}
cand_matches=[]
for candidate in candidates:
matched_colors=[]
info_cand= info_dict[candidate][0] # only first solid in a part
# print ("info_cand=",info_cand)
for color in assy_color_center_area:
assy_area= assy_color_center_area[color]["area"]
# print ("color: %s, assy_area = %f"%(str(color), assy_area))
try:
part_area = info_cand ["colorCenters"][color]["area"]
# print ("color: %s, part_area = %f"%(str(color), part_area))
if abs(part_area - assy_area) < precision * assy_area:
matched_colors.append(color)
except:
pass
cand_matches.append(matched_colors)
max_match = max([len(a) for a in cand_matches])
for candidate, colors in zip(candidates,cand_matches):
if len(colors) == max_match:
colored_candidates[candidate]=colors
return colored_candidates
def findComponents(assembly,
precision_area = PRECISION_AREA,
precision_volume = PRECISION_VOLUME,
precision_gyration = PRECISION_GYRATION,
precision_inside = PRECISION_INSIDE,
precision = PRECISION,
show_best = True):
"""
Match each assembly element with a part, provide the transformation matrix
@param assembly - may be file path (different treatment for Gui/no-Gui, Shape or doc.Objects or "" - will use ActiveDocument().Objects
@param precision_area = PRECISION_AREA - relative precision in surface area calculations
@param precision_volume = PRECISION_VOLUME - relative precision in volume calculations
@param precision_gyration = PRECISION_GYRATION - relative precision in radius of gyration calculations
@param precision_inside = PRECISION_INSIDE - relative precision in calculations of point inside/outside of a solid
@param precision = PRECISION - precision in vector calculation
@param show_best - calculate and show the best relative match for each parameter - can be used to fine-tune PRECISION* parameters
@return a dictionary with 4 fields (each list value has the same number of elements):
'solids' - a list of solids in the assembly
'objects' - a list of solid properties (as dictionaries) used for identification
'candidates' - a list of candidate parts dictionaries, containing lists of matched colors
'transformation' - a list of dictionaries of transformations, indexed by part names (normally just one element)
The same return dictionary is saved as a global variable COMPONENTS and is available as getComponents() method
"""
FreeCAD.Console.PrintMessage("findComponents(): Getting parts database");
global COMPONENTS
start_time=time.time()
print("Getting parts database")
info_dict = get_info_files()
FreeCAD.Console.PrintMessage("findComponents(): Got parts database");
aname = ""
if not assembly: # including "" string
assembly = FreeCAD.activeDocument().Objects
FreeCAD.Console.PrintMessage("Using %d solids in the active document @%f"%(len(assembly), time.time()-start_time));
if isinstance (assembly, (str,unicode)):
assembly_path = assembly
aname,_ = os.path.splitext(os.path.basename(assembly_path))
if not "Gui" in dir(FreeCAD):
print("Reading assembly file %s @%f"%(assembly_path, time.time()-start_time), end="...")
assembly = Part.read(assembly_path)
print(" got %d solids @%f"%(len(assembly.Solids), time.time()-start_time))
else:
FreeCAD.Console.PrintMessage("Using STEP file assembly %s @%f"%(assembly_path, time.time()-start_time));
FreeCAD.loadFile(assembly_path)
doc = FreeCAD.activeDocument()
doc.Label = aname
print(" got %d objects @%f"%(len(doc.Objects), time.time()-start_time))
assembly = doc.Objects
FreeCAD.Console.PrintMessage(" got %d solids @%f"%(len(assembly), time.time()-start_time));
# assuming assembly is doc.Objects
if isinstance(assembly,Part.Shape):
FreeCAD.Console.PrintMessage("Using provided objects @%f"%(len(assembly.Solids), time.time()-start_time));
objects,solids = create_file_info_nogui(shape, aname)
# shape = assembly
else:
objects,solids = create_file_info(assembly, aname)
# print (objects)
progress_bar = Base.ProgressIndicator()
progress_bar.start("Looking for matching parts for each of the assembly element ...", len(objects))
candidates=[]
for i,o in enumerate(objects):
# try:
# FreeCADGui.updateGui()
# except:
# pass
print (i,o)
this_candidates = []
list_errors=[]
rg=o['principal']['RadiusOfGyration']
rg_av = math.sqrt(rg[0]**2 + rg[1]**2 + rg[2]**2)
rgp = precision_gyration * rg_av
vp = o['volume']*precision_volume
ap = o['area']*precision_area
for n in info_dict:
co = info_dict[n][0]
errors = (abs(o['volume'] - co['volume']),
abs(o['area'] - co['area']),
abs(rg[0] - co['principal']['RadiusOfGyration'][0]),
abs(rg[1] - co['principal']['RadiusOfGyration'][1]),
abs(rg[2] - co['principal']['RadiusOfGyration'][2]),
)
if show_best:
list_errors.append(errors)
if ((errors[0] < vp) and
(errors[1] < ap) and
(errors[2] < rgp) and
(errors[3] < rgp) and
(errors[4] < rgp)):
this_candidates.append(n)
if show_best:
weighted_errors = [errors[0]/vp + errors[1]/ap + (errors[2] + errors[3] + errors[4])/rgp for errors in list_errors]
best_index = weighted_errors.index(min(weighted_errors))
errors = list_errors[best_index]
print ("Best match with %s, relative errors: dV=%f, dS=%f, dRG1=%f, dRG2=%f, dRG3=%f"%(
info_dict.keys()[best_index],
errors[0]/o['volume'],
errors[1]/o['area'],
errors[2]/rg_av,
errors[3]/rg_av,
errors[4]/rg_av))
# Filter candidates by number of color areas matched
colored_candidates=colorMatchCandidate(o, this_candidates, info_dict, precision_area)
try:
num_ass_obj_colors = len(o["colorCenters"])
except:
num_ass_obj_colors = 0
print ("%d :colors: %d candidates: %s, colored_candidates: %s"%(i,num_ass_obj_colors, str(this_candidates), str(colored_candidates)))
candidates.append(colored_candidates)
progress_bar.next() # True) # True - enable ESC to abort
progress_bar.stop()
transformations = findPartsTransformations(solids, objects, candidates, info_dict, precision_inside, precision)
#Each part can be in two orientations - check overlap after loading actual parts
COMPONENTS = {"solids":solids,"objects":objects,"candidates":candidates,"transformations":transformations}
return COMPONENTS
# return {"solids":solids,"objects":objects,"candidates":candidates,"transformations":transformations}
def getComponents():
"""
@return global COMPONENTS directory, set by findComponents()
"""
return COMPONENTS
def ppToMatrix(pp,
center = (0,0,0),
colorCenters = {}, # should have all the colors in a colors list "by design"
colors = [],
orient = 0,
precision = PRECISION):
"""
Generates object transformation matix (used for parts and assembly objects) including center of volume translation
and rotational axes (ortho-normal). The axes selection is base on the off-center colored components (centers of same
colored faces) and gyration axes. The 'color' axes have precedence, gyration ones are added when the color ones are
insufficient. First axis is selected as being the longest, second - as having largest component perpendicular to the
first, and the third is just a common perpendicular to the first two. Gyration axes do not provide sign, so for
asymmetrical object with 3 different gyration radii there could be 4 different orientations having the same inertial
properties
@param pp - PrincipalProperties (including gyration axes)
@param center - Center of volume
@param colorCenters - dictionary indexed by colors, having center of color and area of each color (not used here)
@param colors - list of matched colors (tuples)
@param orient - 2-bit modifier for first and second axis of inertia (bit 0 - sign of the first axis, bit 1 - sign of the second)
orient will be overridden if there are some color vectors that define orientation
@param precision - multiplier for the radius of gyration to compare with color vectors
@return 4x4 transformation matrix
"""
rg= pp['RadiusOfGyration']
eps=math.sqrt(rg[0]**2 + rg[1]**2 + rg[2]**2) * precision
color_vectors = []
t = FreeCAD.Vector(center)
vectors=[]
for color in colors:
## print ("colorCenters=", colorCenters[color]['center']," area=",colorCenters[color]['area'])
color_vectors.append(FreeCAD.Vector(colorCenters[color]['center']) - t)
print ("color_vectors=",color_vectors)
## print ("color_vectors=",color_vectors, "t=",t)
if color_vectors: # find the longest one
lengths = [v.Length for v in color_vectors]
l = max(lengths)
v = color_vectors.pop(lengths.index(l))
if l > eps:
vectors.append(v.normalize())
if vectors and color_vectors: # now find the vector having maximal orthogonal component to v[0]
lengths = [v.cross(vectors[0]).Length for v in color_vectors]
l = max(lengths)
v = color_vectors.pop(lengths.index(l))
if l > eps:
vectors.append(v.normalize())
# print ("vectors=",vectors)
#use gyro axis (or two of them)
if len(vectors) < 3: #insufficient color vectors
vgyro=[FreeCAD.Vector(pp["FirstAxisOfInertia"]),
FreeCAD.Vector(pp["SecondAxisOfInertia"]),
FreeCAD.Vector(pp["ThirdAxisOfInertia"])]
if (orient & 1 ) :
vgyro[0].multiply(-1.0)
if (orient & 2 ) :
vgyro[1].multiply(-1.0)
if vgyro[2].dot(vgyro[0].cross(vgyro[1])) < 0 :
vgyro[2].multiply(-1.0)
## print ("vgyro=", vgyro)
if not vectors:
vectors = [vgyro[0], vgyro[1], vgyro[2]]
else: # at least one vector is defined from colors, need one more
new_directions = [False,False,False]
new_length = len(vectors)
if len(vectors) < 2: # == 1, need one more
## print ("vgyro=",vgyro)
for i in range(3): # filter parallel to existing
for v in vectors:
if v.cross(vgyro[i]).Length < eps:
break
else:
new_directions[i] = True
new_length += 1
## print ("new_directions=",new_directions," new_length=",new_length)
if new_length > 2: # extras, filter more (perpendicular to axis of symmetry)
if (new_directions[0] or new_directions[1]) and ((rg[0] - rg[1]) < eps):
if new_directions[1]:
new_directions[1] = False
new_length -= 1
if new_directions[0] and (new_length > 2):
new_directions[0] = False
new_length -= 1
if (new_length > 2) and (new_directions[1] or new_directions[2]) and ((rg[1] - rg[2]) < eps):
if new_directions[1]:
new_directions[1] = False
new_length -= 1
if new_directions[2] and (new_length > 3):
new_directions[2] = False
new_length -= 1
## print ("new_directions=",new_directions," new_length=",new_length)
# All good, add 1,2,3-rd and make ortho-normal
if len(vectors) < 2:
i = new_directions.index(True)
vectors.append((vgyro[i] - vectors[0] * vectors[0].dot(vgyro[i])).normalize())
# here we have 2 vectors, make a third
vectors=[vectors[0],vectors[1], vectors[0].cross(vectors[1]).normalize()]
if vectors[2].dot(vectors[0].cross(vectors[1])) < 0 :
vectors[2].multiply(-1.0)
# print ("Final vectors=",vectors)
return FreeCAD.Matrix(vectors[0].x, vectors[1].x, vectors[2].x, t.x,
vectors[0].y, vectors[1].y, vectors[2].y, t.y,
vectors[0].z, vectors[1].z, vectors[2].z, t.z,
0.0, 0.0, 0.0, 1.0)
def list_parts_offsets():
"""
Shows center of volume distance from (0,0,0) for each part. It may be beneficial
to re-export STEP models from CAD if the offset is very large to increase the
precision of calculations.
Builds the parts info files if not available/obsolete
@return ordered list of (part_name, offset) tuples, in descending order of offsets
"""
info_files = get_info_files()
parts_offsets=[]
for i, name in enumerate(info_files):
for j,o in enumerate(info_files[name]):
d = math.sqrt(o["center"][0]**2 + o["center"][1]**2 + o["center"][2]**2)
if j == 0:
print("%4d:"%(i), end="")
parts_offsets.append((name, d))
else:
print(" ", end="")
print("%s offset = %6.1f"%(name, d))
# now sort:
parts_offsets = sorted(parts_offsets, key=lambda offs: -offs[1])
print ("\nSorted:")
for o in parts_offsets:
print ("%s: %f"%(o))
return parts_offsets
def list_parts():
"""
Output each part information to console/log file
"""
info_files = get_info_files()
for i, name in enumerate(info_files):
print ("%4d '%s': %d solids:%s"%(i,name,len(info_files[name]),str(info_files[name])))
def getShapeNode(vertices, faces, diffuseColor = None, main_color_index = 0, colorPerVertex = False):
"""
Build a node for the tesselated mesh data
@param vertices: list of vertice coordinates as `Vector` type
@param faces: list of tuples of vertice indices and optionally a face color index ex: (1, 2, 3) or (1, 2, 3, 0)
@param diffuseColor: None or a list with 3*N color component values in the form of [R, G, B, R1, G1, B1, ...]
If only 3 color components are specified, they are applied to the whole shape, otherwise each vertex
(or face) is assigned color from the face color index
@param main_color_index - in multi-color object this index sets the color of the object
@param colorPerVertex - True: specify color per erach vertex, False - for each face (reduces file size)
@return XML node for the whole shape to be inserted in the X3D file
"""
shapeNode = et.Element('Shape')
faceNode = et.SubElement(shapeNode, 'IndexedFaceSet')
faceNode.set('coordIndex', ' '.join(["%d %d %d -1" % face[0:3] for face in faces]))
if diffuseColor and (len(diffuseColor) > 3): # Multi-color
if not colorPerVertex:
faceNode.set('colorPerVertex', 'false')
faceNode.set('colorIndex', ' '.join(["%d"%(f[3]) for f in faces]))
else:
faceNode.set('colorIndex', ' '.join(["%d %d %d -1"%(f[3],f[3],f[3]) for f in faces]))
coordinateNode = et.SubElement(faceNode, 'Coordinate')
coordinateNode.set('point',' '.join(["%f %f %f" % (p.x, p.y, p.z) for p in vertices]))
if diffuseColor:
if len(diffuseColor) > 3:
colorNode = et.SubElement(faceNode, 'Color')
colorNode.set('color',' '.join(["%f" % (c) for c in diffuseColor]))
appearanceNode = et.SubElement(shapeNode, 'Appearance')
materialNode = et.SubElement(appearanceNode, 'Material')
materialNode.set('diffuseColor', "%f %f %f" % tuple(diffuseColor[main_color_index * 3: main_color_index * 3 + 3]))
return shapeNode
def exportX3D(objects, filepath, partName="", bbox = None, colorPerVertex=False):
"""
Export given list of objects to a X3D file.
@param objects - a list of dictionaries in the following format:
{
points : [Vector, Vector...],
faces : [(pi, pi, pi, ci), ...], # pi: point index, ci - color index (optional)
color : [R, G, B,...] # number range is 0-1.0, exactly 3 elements for a single color, 3*N for per-vertex colors
}
@param filepath - os path of the file to save X3D data
@param id - id set for the X3D Group node wrapping all the objects in the file
@param bbox - optional bound box as a dictionary {'center':(xc,yc,zc), size:(xs,ys,zs)}
@param colorPerVertex - True: specify color per erach vertex, False - for each face (reduces file size)
"""
progress_bar = Base.ProgressIndicator()
progress_bar.start("Saving objects to X3D file %s ..."%(filepath), len(objects))
x3dNode = et.Element('x3d')
x3dNode.set('profile', 'Interchange')
x3dNode.set('version', '3.3')
sceneNode = et.SubElement(x3dNode, 'Scene')
transformNode =et.SubElement(sceneNode, 'Transform') # Empty transform to adjust center for offset models (use bboxCenter)
transformNode.set('id', 'transformTop_'+partName)
transformNode.set('class', 'transformTop_'+partName)
# if bbox:
# transformNode.set('translation','%f %f %f'%(-bbox['center'][0],-bbox['center'][1],-bbox['center'][2]))
# else:
transformNode.set('translation','%f %f %f'%(0,0,0))
transformNode.set('rotation','%f %f %f %f'%(0,0,0,0))
groupNode = et.SubElement(transformNode, 'Group')
groupNode.set('id', 'groupTop_'+partName)
groupNode.set('class', 'groupTop_'+partName)
if bbox:
groupNode.set('bboxSize','%f %f %f'%bbox['size'])
groupNode.set('bboxCenter','%f %f %f'%bbox['center'])
for o in objects:
shapeNode = getShapeNode(o["points"], o["faces"], o["color"], o["main_color_index"], colorPerVertex)
groupNode.append(shapeNode)
progress_bar.next() # True) # True - enable ESC to abort
oneliner= et.tostring(x3dNode)
reparsed = minidom.parseString(oneliner)
with open(filepath, "wr") as f:
f.write(reparsed.toprettyxml(indent=" "))
progress_bar.stop()
def prepareX3dExport(freecadObjects, fname=""):
"""
Convert object geometry (including color that is separate in FreeCAD) for exporting to X3D,
tessellate faces to traingles
@param freecadObjects - a list of FreeCAD objects
@param fname - file name/path used for the progress bar indicator
@return a list of dictionaries in the following format:
{
points : [Vector, Vector...],
faces : [(pi, pi, pi, ci), ...], # pi: point index, ci - color index (optional)
color : [R, G, B,...] # number range is 0-1.0, exactly 3 elements for a single color, 3*N for per-vertex colors
}
"""
objects = []
progress_bar = Base.ProgressIndicator()
txt=""
if fname:
txt += " in "+fname
progress_bar.start("Generating objects%s to export to X3D ..."%(txt), len(freecadObjects))
xyzMin=None
xyzMax=None
for o in freecadObjects:
progress_bar.next() # True) # have to do it here as 'for' uses 'continue', True - enable ESC to abort
if (not o.ViewObject) or (o.ViewObject.Visibility):
if hasattr(o, "Shape"):
color_set=set()
if o.ViewObject:
for clr in o.ViewObject.DiffuseColor:
color_set.add(clr)
if (len(color_set)>1): # process multi-color objects
col_list = list(color_set)
col_dict={} # index for each color (reverse to list)
for i, clr in enumerate(col_list):
col_dict[clr] = i
points = [] # common for all faces
faces = [] # flat list
colors=[]
color_areas = [0.0] * len(col_list)
for c in col_list:
colors += c[0:3] # only 3 first elements of 4
for i,f in enumerate(o.Shape.Faces):
mesh = f.tessellate(1)
if (not mesh[0]) or (not mesh[1]):
continue # some objects (such as Part:Circle)
color_index = col_dict[o.ViewObject.DiffuseColor[i]] #sometimes len(o.ViewObject.DiffuseColor[i]) ==1, but it will not get here
color_areas [color_index] += f.Area
delta = len(points)
new_indices=[]
for tf in mesh[1]:
new_indices.append((tf[0]+delta,tf[1]+delta,tf[2]+delta, color_index)) # last element - color index
faces += new_indices
points += mesh[0]
#find color with maximal area (will use in "Appearance")
main_color_index = color_areas.index(max(color_areas))
objects.append({
"points": points,
"faces": faces, # Here - 2-d list of tuples
"color": colors, # colors is a list of 3*n elements (n>1)
"main_color_index": main_color_index