Newer
Older
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
import numpy as np
from utils.files.input import ScannedObject
from utils.math.data_extraction import get_mean
def get_mass_properties(obj:ScannedObject):
'''
Evaluate and return a tuple with the following elements:
- the volume
- the position of the center of gravity (COG)
- the inertia matrix expressed at the COG
Documentation can be found here:
http://www.geometrictools.com/Documentation/PolyhedralMassProperties.pdf
'''
verts = np.asarray(obj.get_vertices())
faces = np.asarray(obj.get_faces())
faces = verts[faces]
x = np.asarray([[point[0] for point in faces] for faces in faces])
y = np.asarray([[point[1] for point in faces] for faces in faces])
z = np.asarray([[point[2] for point in faces] for faces in faces])
def subexpression(x):
w0, w1, w2 = x[:, 0], x[:, 1], x[:, 2]
temp0 = w0 + w1
f1 = temp0 + w2
temp1 = w0 * w0
temp2 = temp1 + w1 * temp0
f2 = temp2 + w2 * f1
f3 = w0 * temp1 + w1 * temp2 + w2 * f2
g0 = f2 + w0 * (f1 + w0)
g1 = f2 + w1 * (f1 + w1)
g2 = f2 + w2 * (f1 + w2)
return f1, f2, f3, g0, g1, g2
x0, x1, x2 = x[:, 0], x[:, 1], x[:, 2]
y0, y1, y2 = y[:, 0], y[:, 1], y[:, 2]
z0, z1, z2 = z[:, 0], z[:, 1], z[:, 2]
a1, b1, c1 = x1 - x0, y1 - y0, z1 - z0
a2, b2, c2 = x2 - x0, y2 - y0, z2 - z0
d0, d1, d2 = b1 * c2 - b2 * c1, a2 * c1 - a1 * c2, a1 * b2 - a2 * b1
f1x, f2x, f3x, g0x, g1x, g2x = subexpression(x)
f1y, f2y, f3y, g0y, g1y, g2y = subexpression(y)
f1z, f2z, f3z, g0z, g1z, g2z = subexpression(z)
intg = np.zeros((10))
intg[0] = sum(d0 * f1x)
intg[1:4] = sum(d0 * f2x), sum(d1 * f2y), sum(d2 * f2z)
intg[4:7] = sum(d0 * f3x), sum(d1 * f3y), sum(d2 * f3z)
intg[7] = sum(d0 * (y0 * g0x + y1 * g1x + y2 * g2x))
intg[8] = sum(d1 * (z0 * g0y + z1 * g1y + z2 * g2y))
intg[9] = sum(d2 * (x0 * g0z + x1 * g1z + x2 * g2z))
intg /= np.array([6, 24, 24, 24, 60, 60, 60, 120, 120, 120])
volume = intg[0]
cog = intg[1:4] / volume
cogsq = cog ** 2
inertia = np.zeros((3, 3))
inertia[0, 0] = intg[5] + intg[6] - volume * (cogsq[1] + cogsq[2])
inertia[1, 1] = intg[4] + intg[6] - volume * (cogsq[2] + cogsq[0])
inertia[2, 2] = intg[4] + intg[5] - volume * (cogsq[0] + cogsq[1])
inertia[0, 1] = inertia[1, 0] = -(intg[7] - volume * cog[0] * cog[1])
inertia[1, 2] = inertia[2, 1] = -(intg[8] - volume * cog[1] * cog[2])
inertia[0, 2] = inertia[2, 0] = -(intg[9] - volume * cog[2] * cog[0])
return volume, cog, inertia
def verticalise(obj:ScannedObject):
cog = get_mass_properties(obj)
cog, inertia = get_mass_properties(obj)[1:]
[val,vect] = np.linalg.eig(inertia)
if np.linalg.det(vect) < 0:
vect[:,2] = - vect[:,2]
rot = vect.T
faces = obj.get_faces(True)
theta = -np.pi/2
R_y = np.array([[np.cos(theta), 0, np.sin(theta)],
[0, 1, 0],
[-np.sin(theta), 0, np.cos(theta)]])
for face,_ in enumerate(faces):
for vertex in range(3):
faces[face][vertex] = rot@(faces[face][vertex] - cog)
faces[face][vertex] = R_y@(faces[face][vertex])
obj.update_from_faces(faces)