Home	Trees	Indices	Help	Python File Format Interface

Source Code for Module pyffi.utils.inertia

  1  """Calculate the mass, center of gravity, and inertia matrix for common 
  2  shapes.""" 
  3   
  4  # ***** BEGIN LICENSE BLOCK ***** 
  5  # 
  6  # Copyright (c) 2007-2011, Python File Format Interface 
  7  # All rights reserved. 
  8  # 
  9  # Redistribution and use in source and binary forms, with or without 
 10  # modification, are permitted provided that the following conditions 
 11  # are met: 
 12  # 
 13  #    * Redistributions of source code must retain the above copyright 
 14  #      notice, this list of conditions and the following disclaimer. 
 15  # 
 16  #    * Redistributions in binary form must reproduce the above 
 17  #      copyright notice, this list of conditions and the following 
 18  #      disclaimer in the documentation and/or other materials provided 
 19  #      with the distribution. 
 20  # 
 21  #    * Neither the name of the Python File Format Interface 
 22  #      project nor the names of its contributors may be used to endorse 
 23  #      or promote products derived from this software without specific 
 24  #      prior written permission. 
 25  # 
 26  # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 
 27  # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 
 28  # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 
 29  # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 
 30  # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 
 31  # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 
 32  # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 
 33  # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 
 34  # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 
 35  # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 
 36  # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 
 37  # POSSIBILITY OF SUCH DAMAGE. 
 38  # 
 39  # ***** END LICENSE BLOCK ***** 
 40   
 41  import math 
 42  from pyffi.utils.mathutils import * 
 43   
 44  # see http://en.wikipedia.org/wiki/List_of_moment_of_inertia_tensors 
 45   
 46 -def getMassInertiaSphere(radius, density = 1, solid = True): 
 47      """Return mass and inertia matrix for a sphere of given radius and 
 48      density. 
 49   
 50      >>> mass, inertia_matrix = getMassInertiaSphere(2.0, 3.0) 
 51      >>> mass # doctest: +ELLIPSIS 
 52      100.53096... 
 53      >>> inertia_matrix[0][0] # doctest: +ELLIPSIS 
 54      160.84954...""" 
 55      if solid: 
 56          mass = density * (4 * math.pi * (radius ** 3)) / 3 
 57          inertia = (2 * mass * (radius ** 2)) / 5 
 58      else: 
 59          mass = density * 4 * math.pi * (radius ** 2) 
 60          inertia = (2 * mass * (radius ** 2)) / 3 
 61   
 62      return mass, tuple( tuple( (inertia if i == j else 0) 
 63                                 for i in xrange(3) ) 
 64                          for j in xrange(3) ) 
 65   
 66 -def getMassInertiaBox(size, density = 1, solid = True): 
 67      """Return mass and inertia matrix for a box of given size and 
 68      density. 
 69   
 70      >>> mass, inertia = getMassInertiaBox((1.0, 2.0, 3.0), 4.0) 
 71      >>> mass 
 72      24.0 
 73      >>> inertia 
 74      ((26.0, 0, 0), (0, 20.0, 0), (0, 0, 10.0)) 
 75      """ 
 76      assert(len(size) == 3) # debug 
 77      if solid: 
 78          mass = density * size[0] * size[1] * size[2] 
 79          tmp = tuple(mass * (length ** 2) / 12.0 for length in size) 
 80      else: 
 81          mass = density * sum( x * x for x in size) 
 82          tmp = tuple(mass * (length ** 2) / 6.0 for length in size) # just guessing here, todo calculate it 
 83      return mass, ( ( tmp[1] + tmp[2], 0, 0 ), 
 84                     ( 0, tmp[2] + tmp[0], 0 ), 
 85                     ( 0, 0, tmp[0] + tmp[1] ) ) 
 86   
 87 -def getMassInertiaCapsule(length, radius, density = 1, solid = True): 
 88      # cylinder + caps, and caps have volume of a sphere 
 89      if solid: 
 90          mass = density * (length * math.pi * (radius ** 2) 
 91                            + (4 * math.pi * (radius ** 3)) / 3) 
 92   
 93          # approximate by cylinder 
 94          # TODO: also include the caps into the inertia matrix 
 95          inertia_xx = mass * (3 * (radius ** 2) + (length ** 2)) / 12.0 
 96          inertia_yy = inertia_xx 
 97          inertia_zz = 0.5 * mass * (radius ** 2) 
 98      else: 
 99          mass = density * (length * 2 * math.pi * radius 
100                            + 2 * math.pi * (radius ** 2)) 
101          inertia_xx = mass * (6 * (radius ** 2) + (length ** 2)) / 12.0 
102          inertia_yy = inertia_xx 
103          inertia_zz = mass * (radius ** 2) 
104   
105      return mass,  ( ( inertia_xx, 0, 0 ), 
106                      ( 0, inertia_yy, 0 ), 
107                      ( 0, 0, inertia_zz ) ) 
108   
109  # 
110  # References 
111  # ---------- 
112  # 
113  # Jonathan Blow, Atman J Binstock 
114  # "How to find the inertia tensor (or other mass properties) of a 3D solid body represented by a triangle mesh" 
115  # http://number-none.com/blow/inertia/bb_inertia.doc 
116  # 
117  # David Eberly 
118  # "Polyhedral Mass Properties (Revisited)" 
119  # http://www.geometrictools.com//LibPhysics/RigidBody/Wm4PolyhedralMassProperties.pdf 
120  # 
121  # The function is an implementation of the Blow and Binstock algorithm, 
122  # extended for the case where the polygon is a surface (set parameter 
123  # solid = False). 
124 -def get_mass_center_inertia_polyhedron(vertices, triangles, density = 1, solid = True): 
125      """Return mass, center of gravity, and inertia matrix for a polyhedron. 
126   
127      >>> from pyffi.utils.quickhull import qhull3d 
128      >>> box = [(0,0,0),(1,0,0),(0,2,0),(0,0,3),(1,2,0),(0,2,3),(1,0,3),(1,2,3)] 
129      >>> vertices, triangles = qhull3d(box) 
130      >>> mass, center, inertia = get_mass_center_inertia_polyhedron( 
131      ...     vertices, triangles, density = 4) 
132      >>> mass 
133      24.0 
134      >>> center 
135      (0.5, 1.0, 1.5) 
136      >>> inertia 
137      ((26.0, 0.0, 0.0), (0.0, 20.0, 0.0), (0.0, 0.0, 10.0)) 
138      >>> poly = [(3,0,0),(0,3,0),(-3,0,0),(0,-3,0),(0,0,3),(0,0,-3)] # very rough approximation of a sphere of radius 2 
139      >>> vertices, triangles = qhull3d(poly) 
140      >>> mass, center, inertia = get_mass_center_inertia_polyhedron( 
141      ...     vertices, triangles, density = 3) 
142      >>> mass 
143      108.0 
144      >>> center 
145      (0.0, 0.0, 0.0) 
146      >>> abs(inertia[0][0] - 194.4) < 0.0001 
147      True 
148      >>> abs(inertia[1][1] - 194.4) < 0.0001 
149      True 
150      >>> abs(inertia[2][2] - 194.4) < 0.0001 
151      True 
152      >>> abs(inertia[0][1]) < 0.0001 
153      True 
154      >>> abs(inertia[0][2]) < 0.0001 
155      True 
156      >>> abs(inertia[1][2]) < 0.0001 
157      True 
158      >>> sphere = [] 
159      >>> N = 10 
160      >>> for j in range(-N+1, N): 
161      ...     theta = j * 0.5 * math.pi / N 
162      ...     st, ct = math.sin(theta), math.cos(theta) 
163      ...     M = max(3, int(ct * 2 * N + 0.5)) 
164      ...     for i in range(0, M): 
165      ...         phi = i * 2 * math.pi / M 
166      ...         s, c = math.sin(phi), math.cos(phi) 
167      ...         sphere.append((2*s*ct, 2*c*ct, 2*st)) # construct sphere of radius 2 
168      >>> sphere.append((0,0,2)) 
169      >>> sphere.append((0,0,-2)) 
170      >>> vertices, triangles = qhull3d(sphere) 
171      >>> mass, center, inertia = get_mass_center_inertia_polyhedron( 
172      ...     vertices, triangles, density = 3, solid = True) 
173      >>> abs(mass - 100.53) < 10 # 3*(4/3)*pi*2^3 = 100.53 
174      True 
175      >>> sum(abs(x) for x in center) < 0.01 # is center at origin? 
176      True 
177      >>> abs(inertia[0][0] - 160.84) < 10 
178      True 
179      >>> mass, center, inertia = get_mass_center_inertia_polyhedron( 
180      ...     vertices, triangles, density = 3, solid = False) 
181      >>> abs(mass - 150.79) < 10 # 3*4*pi*2^2 = 150.79 
182      True 
183      >>> abs(inertia[0][0] - mass*0.666*4) < 20 # m*(2/3)*2^2 
184      True 
185      """ 
186   
187      # 120 times the covariance matrix of the canonical tetrahedron 
188      # (0,0,0),(1,0,0),(0,1,0),(0,0,1) 
189      # integrate(integrate(integrate(z*z, x=0..1-y-z), y=0..1-z), z=0..1) = 1/120 
190      # integrate(integrate(integrate(y*z, x=0..1-y-z), y=0..1-z), z=0..1) = 1/60 
191      covariance_canonical = ( (2, 1, 1), 
192                               (1, 2, 1), 
193                               (1, 1, 2) ) 
194      covariance_correction = 1.0/120 
195   
196      covariances = [] 
197      masses = [] 
198      centers = [] 
199   
200      # for each triangle 
201      # construct a tetrahedron from triangle + (0,0,0) 
202      # find its matrix, mass, and center (for density = 1, will be corrected at 
203      # the end of the algorithm) 
204      for triangle in triangles: 
205          # get vertices 
206          vert0, vert1, vert2 = operator.itemgetter(*triangle)(vertices) 
207   
208          # construct a transform matrix that converts the canonical tetrahedron 
209          # into (0,0,0),vert0,vert1,vert2 
210          transform_transposed = ( vert0, vert1, vert2 ) 
211          transform = matTransposed(transform_transposed) 
212   
213          # find the covariance matrix of the transformed tetrahedron/triangle 
214          if solid: 
215              # we shall be needing the determinant more than once, so 
216              # precalculate it 
217              determinant = matDeterminant(transform) 
218              # C' = det(A) * A * C * A^T 
219              covariances.append( 
220                  matscalarMul( 
221                      matMul(matMul(transform, 
222                                    covariance_canonical), 
223                             transform_transposed), 
224                      determinant)) 
225              # m = det(A) / 6.0 
226              masses.append(determinant / 6.0) 
227              # find center of gravity of the tetrahedron 
228              centers.append(tuple( 0.25 * sum(vert[i] 
229                                               for vert in (vert0, vert1, vert2)) 
230                                    for i in xrange(3) )) 
231          else: 
232              # find center of gravity of the triangle 
233              centers.append(tuple( sum(vert[i] 
234                                        for vert in (vert0, vert1, vert2)) / 3.0 
235                                    for i in xrange(3) )) 
236              # find mass of triangle 
237              # mass is surface, which is half the norm of cross product 
238              # of two edges 
239              masses.append( 
240                  vecNorm(vecCrossProduct( 
241                      vecSub(vert1, vert0), vecSub(vert2, vert0))) / 2.0) 
242              # find covariance at center of this triangle 
243              # (this is approximate only as it replaces triangle with point mass 
244              # todo: find better way) 
245              covariances.append( 
246                  tuple(tuple( masses[-1]*x*y for x in centers[-1] ) 
247                        for y in centers[-1])) 
248   
249      # accumulate the results 
250      total_mass = sum(masses) 
251      if total_mass == 0: 
252          # dimension is probably badly chosen 
253          #raise ZeroDivisionError("mass is zero (consider calculating inertia with a lower dimension)") 
254          print("WARNING: mass is nearly zero (%f)" % total_mass) 
255          return 0, (0,0,0), ((0,0,0),(0,0,0),(0,0,0)) 
256      # weighed average of centers with masses 
257      total_center = (0, 0, 0) 
258      for center, mass in izip(centers, masses): 
259          total_center = vecAdd(total_center, 
260                                vecscalarMul(center, mass / total_mass)) 
261      # add covariances, and correct the values 
262      total_covariance = ((0,0,0),(0,0,0),(0,0,0)) 
263      for covariance in covariances: 
264          total_covariance = matAdd(total_covariance, covariance) 
265      if solid: 
266          total_covariance = matscalarMul(total_covariance, covariance_correction) 
267   
268      # translate covariance to center of gravity: 
269      # C' = C - m * ( x dx^T + dx x^T + dx dx^T ) 
270      # with x the translation vector and dx the center of gravity 
271      translate_correction = matscalarMul(tuple(tuple(x * y 
272                                                      for x in total_center) 
273                                                for y in total_center), 
274                                          total_mass) 
275      total_covariance = matSub(total_covariance, translate_correction) 
276   
277      # convert covariance matrix into inertia tensor 
278      trace = sum(total_covariance[i][i] for i in xrange(3)) 
279      trace_matrix = tuple(tuple((trace if i == j else 0) 
280                                 for i in xrange(3)) 
281                           for j in xrange(3)) 
282      total_inertia = matSub(trace_matrix, total_covariance) 
283   
284      # correct for given density 
285      total_inertia = matscalarMul(total_inertia, density) 
286      total_mass *= density 
287   
288      # correct negative mass 
289      if total_mass < 0: 
290          total_mass = -total_mass 
291          total_inertia = tuple(tuple(-x for x in row) 
292                                for row in total_inertia) 
293   
294      return total_mass, total_center, total_inertia 
295   
296  if __name__ == "__main__": 
297      import doctest 
298      doctest.testmod() 
299   

Home	Trees	Indices	Help	Python File Format Interface