chiark / gitweb /
Add back the ultimaker platform, and made the platform mesh simpler.
[cura.git] / Cura / slice / cura_sf / fabmetheus_utilities / geometry / geometry_utilities / matrix.py
1 """
2 Boolean geometry four by four matrix.
3
4 """
5
6 from __future__ import absolute_import
7
8 from fabmetheus_utilities.geometry.geometry_utilities import evaluate
9 from fabmetheus_utilities.vector3 import Vector3
10 from fabmetheus_utilities import euclidean
11 from fabmetheus_utilities import xml_simple_writer
12 import cStringIO
13 import math
14
15
16 __author__ = 'Enrique Perez (perez_enrique@yahoo.com)'
17 __credits__ = 'Art of Illusion <http://www.artofillusion.org/>'
18 __date__ = '$Date: 2008/02/05 $'
19 __license__ = 'GNU Affero General Public License http://www.gnu.org/licenses/agpl.html'
20
21
22 globalExecutionOrder = 300
23
24
25 def addVertexes(geometryOutput, vertexes):
26         'Add the vertexes.'
27         if geometryOutput.__class__ == list:
28                 for element in geometryOutput:
29                         addVertexes(element, vertexes)
30                 return
31         if geometryOutput.__class__ == dict:
32                 for geometryOutputKey in geometryOutput.keys():
33                         if geometryOutputKey == 'vertex':
34                                 vertexes += geometryOutput[geometryOutputKey]
35                         else:
36                                 addVertexes(geometryOutput[geometryOutputKey], vertexes)
37
38 def getBranchMatrix(elementNode):
39         'Get matrix starting from the object if it exists, otherwise get a matrix starting from stratch.'
40         branchMatrix = Matrix()
41         matrixChildElement = elementNode.getFirstChildByLocalName('matrix')
42         if matrixChildElement != None:
43                 branchMatrix = branchMatrix.getFromElementNode(matrixChildElement, '')
44         branchMatrix = branchMatrix.getFromElementNode(elementNode, 'matrix.')
45         if elementNode.xmlObject == None:
46                 return branchMatrix
47         elementNodeMatrix = elementNode.xmlObject.getMatrix4X4()
48         if elementNodeMatrix == None:
49                 return branchMatrix
50         return elementNodeMatrix.getOtherTimesSelf(branchMatrix.tetragrid)
51
52 def getBranchMatrixSetElementNode(elementNode):
53         'Get matrix starting from the object if it exists, otherwise get a matrix starting from stratch.'
54         branchMatrix = getBranchMatrix(elementNode)
55         setElementNodeDictionaryMatrix(elementNode, branchMatrix)
56         return branchMatrix
57
58 def getCumulativeVector3Remove(defaultVector3, elementNode, prefix):
59         'Get cumulative vector3 and delete the prefixed attributes.'
60         if prefix == '':
61                 defaultVector3.x = evaluate.getEvaluatedFloat(defaultVector3.x, elementNode, 'x')
62                 defaultVector3.y = evaluate.getEvaluatedFloat(defaultVector3.y, elementNode, 'y')
63                 defaultVector3.z = evaluate.getEvaluatedFloat(defaultVector3.z, elementNode, 'z')
64                 euclidean.removeElementsFromDictionary(elementNode.attributes, ['x', 'y', 'z'])
65                 prefix = 'cartesian'
66         defaultVector3 = evaluate.getVector3ByPrefix(defaultVector3, elementNode, prefix)
67         euclidean.removePrefixFromDictionary(elementNode.attributes, prefix)
68         return defaultVector3
69
70 def getDiagonalSwitchedTetragrid(angleDegrees, diagonals):
71         'Get the diagonals and switched matrix by degrees.'
72         return getDiagonalSwitchedTetragridByRadians(math.radians(angleDegrees), diagonals)
73
74 def getDiagonalSwitchedTetragridByPolar(diagonals, unitPolar):
75         'Get the diagonals and switched matrix by unitPolar.'
76         diagonalSwitchedTetragrid = getIdentityTetragrid()
77         for diagonal in diagonals:
78                 diagonalSwitchedTetragrid[diagonal][diagonal] = unitPolar.real
79         diagonalSwitchedTetragrid[diagonals[0]][diagonals[1]] = -unitPolar.imag
80         diagonalSwitchedTetragrid[diagonals[1]][diagonals[0]] = unitPolar.imag
81         return diagonalSwitchedTetragrid
82
83 def getDiagonalSwitchedTetragridByRadians(angleRadians, diagonals):
84         'Get the diagonals and switched matrix by radians.'
85         return getDiagonalSwitchedTetragridByPolar(diagonals, euclidean.getWiddershinsUnitPolar(angleRadians))
86
87 def getIdentityTetragrid(tetragrid=None):
88         'Get four by four matrix with diagonal elements set to one.'
89         if tetragrid == None:
90                 return [[1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0], [0.0, 0.0, 1.0, 0.0], [0.0, 0.0, 0.0, 1.0]]
91         return tetragrid
92
93 def getIsIdentityTetragrid(tetragrid):
94         'Determine if the tetragrid is the identity tetragrid.'
95         for column in xrange(4):
96                 for row in xrange(4):
97                         if column == row:
98                                 if tetragrid[column][row] != 1.0:
99                                         return False
100                         elif tetragrid[column][row] != 0.0:
101                                 return False
102         return True
103
104 def getIsIdentityTetragridOrNone(tetragrid):
105         'Determine if the tetragrid is None or if it is the identity tetragrid.'
106         if tetragrid == None:
107                 return True
108         return getIsIdentityTetragrid(tetragrid)
109
110 def getKeyA(row, column, prefix=''):
111         'Get the a format key string from row & column, counting from zero.'
112         return '%sa%s%s' % (prefix, row, column)
113
114 def getKeyM(row, column, prefix=''):
115         'Get the m format key string from row & column, counting from one.'
116         return '%sm%s%s' % (prefix, row + 1, column + 1)
117
118 def getKeysA(prefix=''):
119         'Get the matrix keys, counting from zero.'
120         keysA = []
121         for row in xrange(4):
122                 for column in xrange(4):
123                         key = getKeyA(row, column, prefix)
124                         keysA.append(key)
125         return keysA
126
127 def getKeysM(prefix=''):
128         'Get the matrix keys, counting from one.'
129         keysM = []
130         for row in xrange(4):
131                 for column in xrange(4):
132                         key = getKeyM(row, column, prefix)
133                         keysM.append(key)
134         return keysM
135
136 def getRemovedFloat(defaultFloat, elementNode, key, prefix):
137         'Get the float by the key and the prefix.'
138         prefixKey = prefix + key
139         if prefixKey in elementNode.attributes:
140                 floatValue = evaluate.getEvaluatedFloat(None, elementNode, prefixKey)
141                 if floatValue == None:
142                         print('Warning, evaluated value in getRemovedFloatByKeys in matrix is None for key:')
143                         print(prefixKey)
144                         print('for elementNode dictionary value:')
145                         print(elementNode.attributes[prefixKey])
146                         print('for elementNode dictionary:')
147                         print(elementNode.attributes)
148                 else:
149                         defaultFloat = floatValue
150                 del elementNode.attributes[prefixKey]
151         return defaultFloat
152
153 def getRemovedFloatByKeys(defaultFloat, elementNode, keys, prefix):
154         'Get the float by the keys and the prefix.'
155         for key in keys:
156                 defaultFloat = getRemovedFloat(defaultFloat, elementNode, key, prefix)
157         return defaultFloat
158
159 def getRotateAroundAxisTetragrid(elementNode, prefix):
160         'Get rotate around axis tetragrid and delete the axis and angle attributes.'
161         angle = getRemovedFloatByKeys(0.0, elementNode, ['angle', 'counterclockwise'], prefix)
162         angle -= getRemovedFloat(0.0, elementNode, 'clockwise', prefix)
163         if angle == 0.0:
164                 return None
165         angleRadians = math.radians(angle)
166         axis = getCumulativeVector3Remove(Vector3(), elementNode, prefix + 'axis')
167         axisLength = abs(axis)
168         if axisLength <= 0.0:
169                 print('Warning, axisLength was zero in getRotateAroundAxisTetragrid in matrix so nothing will be done for:')
170                 print(elementNode)
171                 return None
172         axis /= axisLength
173         tetragrid = getIdentityTetragrid()
174         cosAngle = math.cos(angleRadians)
175         sinAngle = math.sin(angleRadians)
176         oneMinusCos = 1.0 - math.cos(angleRadians)
177         xx = axis.x * axis.x
178         xy = axis.x * axis.y
179         xz = axis.x * axis.z
180         yy = axis.y * axis.y
181         yz = axis.y * axis.z
182         zz = axis.z * axis.z
183         tetragrid[0] = [cosAngle + xx * oneMinusCos, xy * oneMinusCos - axis.z * sinAngle, xz * oneMinusCos + axis.y * sinAngle, 0.0]
184         tetragrid[1] = [xy * oneMinusCos + axis.z * sinAngle, cosAngle + yy * oneMinusCos, yz * oneMinusCos - axis.x * sinAngle, 0.0]
185         tetragrid[2] = [xz * oneMinusCos - axis.y * sinAngle, yz * oneMinusCos + axis.x * sinAngle, cosAngle + zz * oneMinusCos, 0.0]
186         return tetragrid
187
188 def getRotateTetragrid(elementNode, prefix):
189         'Get rotate tetragrid and delete the rotate attributes.'
190         # http://en.wikipedia.org/wiki/Rotation_matrix
191         rotateMatrix = Matrix()
192         rotateMatrix.tetragrid = getRotateAroundAxisTetragrid(elementNode, prefix)
193         zAngle = getRemovedFloatByKeys(0.0, elementNode, ['axisclockwisez', 'observerclockwisez', 'z'], prefix)
194         zAngle -= getRemovedFloatByKeys(0.0, elementNode, ['axiscounterclockwisez', 'observercounterclockwisez'], prefix)
195         if zAngle != 0.0:
196                 rotateMatrix.tetragrid = getTetragridTimesOther(getDiagonalSwitchedTetragrid(-zAngle, [0, 1]), rotateMatrix.tetragrid)
197         xAngle = getRemovedFloatByKeys(0.0, elementNode, ['axisclockwisex', 'observerclockwisex', 'x'], prefix)
198         xAngle -= getRemovedFloatByKeys(0.0, elementNode, ['axiscounterclockwisex', 'observercounterclockwisex'], prefix)
199         if xAngle != 0.0:
200                 rotateMatrix.tetragrid = getTetragridTimesOther(getDiagonalSwitchedTetragrid(-xAngle, [1, 2]), rotateMatrix.tetragrid)
201         yAngle = getRemovedFloatByKeys(0.0, elementNode, ['axiscounterclockwisey', 'observerclockwisey', 'y'], prefix)
202         yAngle -= getRemovedFloatByKeys(0.0, elementNode, ['axisclockwisey', 'observercounterclockwisey'], prefix)
203         if yAngle != 0.0:
204                 rotateMatrix.tetragrid = getTetragridTimesOther(getDiagonalSwitchedTetragrid(yAngle, [0, 2]), rotateMatrix.tetragrid)
205         return rotateMatrix.tetragrid
206
207 def getScaleTetragrid(elementNode, prefix):
208         'Get scale matrix and delete the scale attributes.'
209         scaleDefaultVector3 = Vector3(1.0, 1.0, 1.0)
210         scale = getCumulativeVector3Remove(scaleDefaultVector3.copy(), elementNode, prefix)
211         if scale == scaleDefaultVector3:
212                 return None
213         return [[scale.x, 0.0, 0.0, 0.0], [0.0, scale.y, 0.0, 0.0], [0.0, 0.0, scale.z, 0.0], [0.0, 0.0, 0.0, 1.0]]
214
215 def getTetragridA(elementNode, prefix, tetragrid):
216         'Get the tetragrid from the elementNode letter a values.'
217         keysA = getKeysA(prefix)
218         evaluatedDictionary = evaluate.getEvaluatedDictionaryByEvaluationKeys(elementNode, keysA)
219         if len(evaluatedDictionary.keys()) < 1:
220                 return tetragrid
221         for row in xrange(4):
222                 for column in xrange(4):
223                         key = getKeyA(row, column, prefix)
224                         if key in evaluatedDictionary:
225                                 value = evaluatedDictionary[key]
226                                 if value == None or value == 'None':
227                                         print('Warning, value in getTetragridA in matrix is None for key for dictionary:')
228                                         print(key)
229                                         print(evaluatedDictionary)
230                                 else:
231                                         tetragrid = getIdentityTetragrid(tetragrid)
232                                         tetragrid[row][column] = float(value)
233         euclidean.removeElementsFromDictionary(elementNode.attributes, keysA)
234         return tetragrid
235
236 def getTetragridC(elementNode, prefix, tetragrid):
237         'Get the matrix Tetragrid from the elementNode letter c values.'
238         columnKeys = 'Pc1 Pc2 Pc3 Pc4'.replace('P', prefix).split()
239         evaluatedDictionary = evaluate.getEvaluatedDictionaryByEvaluationKeys(elementNode, columnKeys)
240         if len(evaluatedDictionary.keys()) < 1:
241                 return tetragrid
242         for columnKeyIndex, columnKey in enumerate(columnKeys):
243                 if columnKey in evaluatedDictionary:
244                         value = evaluatedDictionary[columnKey]
245                         if value == None or value == 'None':
246                                 print('Warning, value in getTetragridC in matrix is None for columnKey for dictionary:')
247                                 print(columnKey)
248                                 print(evaluatedDictionary)
249                         else:
250                                 tetragrid = getIdentityTetragrid(tetragrid)
251                                 for elementIndex, element in enumerate(value):
252                                         tetragrid[elementIndex][columnKeyIndex] = element
253         euclidean.removeElementsFromDictionary(elementNode.attributes, columnKeys)
254         return tetragrid
255
256 def getTetragridCopy(tetragrid):
257         'Get tetragrid copy.'
258         if tetragrid == None:
259                 return None
260         tetragridCopy = []
261         for tetragridRow in tetragrid:
262                 tetragridCopy.append(tetragridRow[:])
263         return tetragridCopy
264
265 def getTetragridM(elementNode, prefix, tetragrid):
266         'Get the tetragrid from the elementNode letter m values.'
267         keysM = getKeysM(prefix)
268         evaluatedDictionary = evaluate.getEvaluatedDictionaryByEvaluationKeys(elementNode, keysM)
269         if len(evaluatedDictionary.keys()) < 1:
270                 return tetragrid
271         for row in xrange(4):
272                 for column in xrange(4):
273                         key = getKeyM(row, column, prefix)
274                         if key in evaluatedDictionary:
275                                 value = evaluatedDictionary[key]
276                                 if value == None or value == 'None':
277                                         print('Warning, value in getTetragridM in matrix is None for key for dictionary:')
278                                         print(key)
279                                         print(evaluatedDictionary)
280                                 else:
281                                         tetragrid = getIdentityTetragrid(tetragrid)
282                                         tetragrid[row][column] = float(value)
283         euclidean.removeElementsFromDictionary(elementNode.attributes, keysM)
284         return tetragrid
285
286 def getTetragridMatrix(elementNode, prefix, tetragrid):
287         'Get the tetragrid from the elementNode matrix value.'
288         matrixKey = prefix + 'matrix'
289         evaluatedDictionary = evaluate.getEvaluatedDictionaryByEvaluationKeys(elementNode, [matrixKey])
290         if len(evaluatedDictionary.keys()) < 1:
291                 return tetragrid
292         value = evaluatedDictionary[matrixKey]
293         if value == None or value == 'None':
294                 print('Warning, value in getTetragridMatrix in matrix is None for matrixKey for dictionary:')
295                 print(matrixKey)
296                 print(evaluatedDictionary)
297         else:
298                 tetragrid = getIdentityTetragrid(tetragrid)
299                 for rowIndex, row in enumerate(value):
300                         for elementIndex, element in enumerate(row):
301                                 tetragrid[rowIndex][elementIndex] = element
302         euclidean.removeElementsFromDictionary(elementNode.attributes, [matrixKey])
303         return tetragrid
304
305 def getTetragridR(elementNode, prefix, tetragrid):
306         'Get the tetragrid from the elementNode letter r values.'
307         rowKeys = 'Pr1 Pr2 Pr3 Pr4'.replace('P', prefix).split()
308         evaluatedDictionary = evaluate.getEvaluatedDictionaryByEvaluationKeys(elementNode, rowKeys)
309         if len(evaluatedDictionary.keys()) < 1:
310                 return tetragrid
311         for rowKeyIndex, rowKey in enumerate(rowKeys):
312                 if rowKey in evaluatedDictionary:
313                         value = evaluatedDictionary[rowKey]
314                         if value == None or value == 'None':
315                                 print('Warning, value in getTetragridR in matrix is None for rowKey for dictionary:')
316                                 print(rowKey)
317                                 print(evaluatedDictionary)
318                         else:
319                                 tetragrid = getIdentityTetragrid(tetragrid)
320                                 for elementIndex, element in enumerate(value):
321                                         tetragrid[rowKeyIndex][elementIndex] = element
322         euclidean.removeElementsFromDictionary(elementNode.attributes, rowKeys)
323         return tetragrid
324
325 def getTetragridTimesOther(firstTetragrid, otherTetragrid ):
326         'Get this matrix multiplied by the other matrix.'
327         #A down, B right from http://en.wikipedia.org/wiki/Matrix_multiplication
328         if firstTetragrid == None:
329                 return otherTetragrid
330         if otherTetragrid == None:
331                 return firstTetragrid
332         tetragridTimesOther = []
333         for row in xrange(4):
334                 matrixRow = firstTetragrid[row]
335                 tetragridTimesOtherRow = []
336                 tetragridTimesOther.append(tetragridTimesOtherRow)
337                 for column in xrange(4):
338                         dotProduct = 0
339                         for elementIndex in xrange(4):
340                                 dotProduct += matrixRow[elementIndex] * otherTetragrid[elementIndex][column]
341                         tetragridTimesOtherRow.append(dotProduct)
342         return tetragridTimesOther
343
344 def getTransformedByList(floatList, point):
345         'Get the point transformed by the array.'
346         return floatList[0] * point.x + floatList[1] * point.y + floatList[2] * point.z + floatList[3]
347
348 def getTransformedVector3(tetragrid, vector3):
349         'Get the vector3 multiplied by a matrix.'
350         if getIsIdentityTetragridOrNone(tetragrid):
351                 return vector3.copy()
352         return getTransformedVector3Blindly(tetragrid, vector3)
353
354 def getTransformedVector3Blindly(tetragrid, vector3):
355         'Get the vector3 multiplied by a tetragrid without checking if the tetragrid exists.'
356         return Vector3(
357                 getTransformedByList(tetragrid[0], vector3),
358                 getTransformedByList(tetragrid[1], vector3),
359                 getTransformedByList(tetragrid[2], vector3))
360
361 def getTransformedVector3s(tetragrid, vector3s):
362         'Get the vector3s multiplied by a matrix.'
363         if getIsIdentityTetragridOrNone(tetragrid):
364                 return euclidean.getPathCopy(vector3s)
365         transformedVector3s = []
366         for vector3 in vector3s:
367                 transformedVector3s.append(getTransformedVector3Blindly(tetragrid, vector3))
368         return transformedVector3s
369
370 def getTransformTetragrid(elementNode, prefix):
371         'Get the tetragrid from the elementNode.'
372         tetragrid = getTetragridA(elementNode, prefix, None)
373         tetragrid = getTetragridC(elementNode, prefix, tetragrid)
374         tetragrid = getTetragridM(elementNode, prefix, tetragrid)
375         tetragrid = getTetragridMatrix(elementNode, prefix, tetragrid)
376         tetragrid = getTetragridR(elementNode, prefix, tetragrid)
377         return tetragrid
378
379 def getTranslateTetragrid(elementNode, prefix):
380         'Get translate matrix and delete the translate attributes.'
381         translation = getCumulativeVector3Remove(Vector3(), elementNode, prefix)
382         if translation.getIsDefault():
383                 return None
384         return getTranslateTetragridByTranslation(translation)
385
386 def getTranslateTetragridByTranslation(translation):
387         'Get translate tetragrid by translation.'
388         return [[1.0, 0.0, 0.0, translation.x], [0.0, 1.0, 0.0, translation.y], [0.0, 0.0, 1.0, translation.z], [0.0, 0.0, 0.0, 1.0]]
389
390 def getVertexes(geometryOutput):
391         'Get the vertexes.'
392         vertexes = []
393         addVertexes(geometryOutput, vertexes)
394         return vertexes
395
396 def setAttributesToMultipliedTetragrid(elementNode, tetragrid):
397         'Set the element attribute dictionary and element matrix to the matrix times the tetragrid.'
398         setElementNodeDictionaryMatrix(elementNode, getBranchMatrix(elementNode).getOtherTimesSelf(tetragrid))
399
400 def setElementNodeDictionaryMatrix(elementNode, matrix4X4):
401         'Set the element attribute dictionary or element matrix to the matrix.'
402         if elementNode.xmlObject == None:
403                 elementNode.attributes.update(matrix4X4.getAttributes('matrix.'))
404         else:
405                 elementNode.xmlObject.matrix4X4 = matrix4X4
406
407 def transformVector3Blindly(tetragrid, vector3):
408         'Transform the vector3 by a tetragrid without checking to see if it exists.'
409         x = getTransformedByList(tetragrid[0], vector3)
410         y = getTransformedByList(tetragrid[1], vector3)
411         z = getTransformedByList(tetragrid[2], vector3)
412         vector3.x = x
413         vector3.y = y
414         vector3.z = z
415
416 def transformVector3ByMatrix(tetragrid, vector3):
417         'Transform the vector3 by a matrix.'
418         if getIsIdentityTetragridOrNone(tetragrid):
419                 return
420         transformVector3Blindly(tetragrid, vector3)
421
422 def transformVector3sByMatrix(tetragrid, vector3s):
423         'Transform the vector3s by a matrix.'
424         if getIsIdentityTetragridOrNone(tetragrid):
425                 return
426         for vector3 in vector3s:
427                 transformVector3Blindly(tetragrid, vector3)
428
429
430 class Matrix(object):
431         'A four by four matrix.'
432         def __init__(self, tetragrid=None):
433                 'Add empty lists.'
434                 self.tetragrid = getTetragridCopy(tetragrid)
435
436         def __eq__(self, other):
437                 'Determine whether this matrix is identical to other one.'
438                 if other == None:
439                         return False
440                 if other.__class__ != self.__class__:
441                         return False
442                 return other.tetragrid == self.tetragrid
443
444         def __ne__(self, other):
445                 'Determine whether this vector is not identical to other one.'
446                 return not self.__eq__(other)
447
448         def __repr__(self):
449                 'Get the string representation of this four by four matrix.'
450                 output = cStringIO.StringIO()
451                 self.addXML(0, output)
452                 return output.getvalue()
453
454         def addXML(self, depth, output):
455                 'Add xml for this object.'
456                 attributes = self.getAttributes()
457                 if len(attributes) > 0:
458                         xml_simple_writer.addClosedXMLTag(attributes, depth, self.__class__.__name__.lower(), output)
459
460         def getAttributes(self, prefix=''):
461                 'Get the attributes from row column attribute strings, counting from one.'
462                 attributes = {}
463                 if self.tetragrid == None:
464                         return attributes
465                 for row in xrange(4):
466                         for column in xrange(4):
467                                 default = float(column == row)
468                                 value = self.tetragrid[row][column]
469                                 if abs( value - default ) > 0.00000000000001:
470                                         if abs(value) < 0.00000000000001:
471                                                 value = 0.0
472                                         attributes[prefix + getKeyM(row, column)] = value
473                 return attributes
474
475         def getFromElementNode(self, elementNode, prefix):
476                 'Get the values from row column attribute strings, counting from one.'
477                 attributes = elementNode.attributes
478                 if attributes == None:
479                         return self
480                 self.tetragrid = getTetragridTimesOther(getTransformTetragrid(elementNode, prefix), self.tetragrid)
481                 self.tetragrid = getTetragridTimesOther(getScaleTetragrid(elementNode, 'scale.'), self.tetragrid)
482                 self.tetragrid = getTetragridTimesOther(getRotateTetragrid(elementNode, 'rotate.'), self.tetragrid)
483                 self.tetragrid = getTetragridTimesOther(getTranslateTetragrid(elementNode, 'translate.'), self.tetragrid)
484                 return self
485
486         def getOtherTimesSelf(self, otherTetragrid):
487                 'Get this matrix reverse multiplied by the other matrix.'
488                 return Matrix(getTetragridTimesOther(otherTetragrid, self.tetragrid))
489
490         def getSelfTimesOther(self, otherTetragrid):
491                 'Get this matrix multiplied by the other matrix.'
492                 return Matrix(getTetragridTimesOther(self.tetragrid, otherTetragrid))