def rotate(u,angle,axis=Vector(0,0,1)):
'''rotate(Vector,Float,axis=Vector): rotates the first Vector
around the given axis, at the given angle.
If axis is omitted, the rotation is made on the xy plane.'''
typecheck ([(u,Vector), (angle,(int,float)), (axis,Vector)], "rotate")
if angle == 0:
return u
l=axis.Length
x=axis.x/l
y=axis.y/l
z=axis.z/l
c = math.cos(angle)
s = math.sin(angle)
t = 1 - c
xyt = x*y*t
xzt = x*z*t
yzt = y*z*t
xs = x*s
ys = y*s
zs = z*s
m = Matrix(c + x*x*t, xyt - zs, xzt + ys, 0,
xyt + zs, c + y*y*t, yzt - xs, 0,
xzt - ys, yzt + xs, c + z*z*t, 0)
return m.multiply(u)
def rotate(u, angle, axis=Vector(0, 0, 1)):
'''rotate(Vector,Float,axis=Vector): rotates the first Vector
around the given axis, at the given angle.
If axis is omitted, the rotation is made on the xy plane.'''
typecheck([(u, Vector), (angle, (int, long, float)), (axis, Vector)],
"rotate")
if angle == 0:
return u
l = axis.Length
x = axis.x / l
y = axis.y / l
z = axis.z / l
c = math.cos(angle)
s = math.sin(angle)
t = 1 - c
xyt = x * y * t
xzt = x * z * t
yzt = y * z * t
xs = x * s
ys = y * s
zs = z * s
m = Matrix(c + x * x * t, xyt - zs, xzt + ys, 0, xyt + zs, c + y * y * t,
yzt - xs, 0, xzt - ys, yzt + xs, c + z * z * t, 0)
return m.multiply(u)
def test_from_matrix(self):
from FreeCAD import Matrix
# identity
self.assertEqual(CoordSystem.from_transform(Matrix()), CoordSystem())
# random #1
m = Matrix(-0.146655, -0.271161, -0.951296, 0.0376659, -0.676234,
0.729359, -0.103649, 0.615421, 0.721942, 0.628098,
-0.290333, -0.451955, 0, 0, 0, 1)
cs = CoordSystem.from_transform(m)
self.assertEqual(
cs,
CoordSystem(
origin=(0.0376659, 0.615421, -0.451955),
xDir=(-0.14665525299526946, -0.6762339076811328,
0.7219417835748246),
normal=(-0.9512957880009034, -0.10364897690151711,
-0.2903329352984416),
))
# random #2
m = Matrix(0.423408, -0.892837, -0.153517, -0.163654, -0.617391,
-0.408388, 0.672345, 0.835824, -0.662989, -0.189896,
-0.724144, 0.632804, 0, 0, 0, 1)
cs = CoordSystem.from_transform(m)
self.assertEqual(
cs,
CoordSystem(
origin=(-0.163654, 0.835824, 0.632804),
xDir=(0.4234078285564432, -0.6173904937335437,
-0.6629892826920875),
normal=(-0.15351701527110584, 0.672345066881529,
-0.7241440720342351),
))
def __init__(self, doc, name='cone'):
self.data = {
'len lo': 70., # mm
'len up': 120., # mm
'int diameter': 36., # mm
'thick': 1., # mm
}
int_diam = self.data['int diameter']
len_up = self.data['len up']
len_lo = self.data['len lo']
ext_diam = int_diam + 2 * self.data['thick']
int_cone = Part.makeSphere(int_diam / 2)
ext_cone = Part.makeSphere(ext_diam / 2)
box = Part.makeBox(ext_diam, ext_diam, ext_diam)
box.translate(Vector(-ext_diam / 2, -ext_diam / 2, 0))
cone_up = ext_cone.cut(int_cone)
cone_up = cone_up.common(box)
matrix = Matrix()
matrix.scale(1., 1., len_up / (ext_diam / 2))
cone_up = cone_up.transformGeometry(matrix)
cone_up.translate(Vector(0, 0, len_lo))
cone_up = cone_up.fuse(cone_up)
# blocking thread (pas de vis de blocage)
# radius = self.data['int diameter'] / 2
#
# helix = Part.makeHelix(4., 20., radius)
#
# p0 = (radius, 0, 0)
# p1 = (radius, 0, 3)
# p2 = (radius - 1, 0, 2)
# p3 = (radius - 1, 0, 1)
#
# e0 = Part.makeLine(p0, p1)
# e1 = Part.makeLine(p1, p2)
# e2 = Part.makeLine(p2, p3)
# e3 = Part.makeLine(p3, p0)
# section = Part.Wire([e0, e1, e2, e3])
# helix = Part.Wire(helix).makePipeShell([section], 1, 1)
# helix.translate(Vector(0, 0, len_lo - 20))
int_tube = Part.makeCylinder(int_diam / 2, len_lo)
ext_tube = Part.makeCylinder(ext_diam / 2, len_lo)
cone_lo = ext_tube.cut(int_tube)
# cone_lo = cone_lo.fuse(helix)
#comp = cone_up.fuse(cone_lo) # BUG: this fusion fails!!!
comp = cone_lo
MecaComponent.__init__(self, doc, comp, name, (1., 1., 0.))
def getMatrix(self):
m = self.m
return Matrix( \
m[0][0], m[0][1], m[0][2], m[0][3], \
m[1][0], m[1][1], m[1][2], m[1][3], \
m[2][0], m[2][1], m[2][2], m[2][3], \
m[3][0], m[3][1], m[3][2], m[3][3] \
)
def __valueAtEllipse__(ra, rb, center, axis, u): x = VEC(cos(u) * ra, sin(u) * rb, 0) theta = axis.getAngle(DIR_Z) if (abs(theta) < 1e-06): n = DIR_X elif (abs(theta - pi) < 1e-06): n = -DIR_X else: n = DIR_Z.cross(axis) if (n.Length == 0): return x n = n.normalize() a = cos(theta/2) b = n.x*sin(theta/2) c = n.y*sin(theta/2) d = n.z*sin(theta/2) aa, bb, cc, dd = a*a, b*b, c*c, d*d bc, ad, ac, ab, bd, cd = b*c, a*d, a*c, a*b, b*d, c*d m = MAT(aa+bb-cc-dd, 2*(bc+ad), 2*(bd-ac), 0, 2*(bc-ad), aa+cc-bb-dd, 2*(cd+ab), 0, 2*(bd+ac), 2*(cd-ab), aa+dd-bb-cc, 0, 0, 0, 0, 1) return center + m.multiply(x)
def getCoG(self,
fp,
vol,
roll=Units.parseQuantity("0 deg"),
trim=Units.parseQuantity("0 deg")):
"""Return the fluid volume center of gravity, provided the volume of
fluid inside the tank.
The returned center of gravity is refered to the untransformed ship.
Keyword arguments:
fp -- Part::FeaturePython object affected.
vol -- Volume of fluid.
roll -- Ship roll angle.
trim -- Ship trim angle.
If the fluid volume is bigger than the total tank one, it will be
conveniently clamped.
"""
# Change the units of the volume, and clamp the value
if vol <= 0.0:
return Vector()
if vol >= fp.Shape.Volume:
vol = 0.0
for solid in fp.Shape.Solids:
vol += solid.Volume
sCoG = solid.CenterOfMass
cog.x = cog.x + sCoG.x * solid.Volume
cog.y = cog.y + sCoG.y * solid.Volume
cog.z = cog.z + sCoG.z * solid.Volume
cog.x = cog.x / vol
cog.y = cog.y / vol
cog.z = cog.z / vol
return cog
# Get a first estimation of the level
level = vol.Value / fp.Shape.Volume
# Transform the tank shape
current_placement = fp.Placement
m = current_placement.toMatrix()
m.rotateX(roll.getValueAs("rad"))
m.rotateY(-trim.getValueAs("rad"))
fp.Placement = Placement(m)
# Iterate to find the fluid shape
for i in range(COMMON_BOOLEAN_ITERATIONS):
shape = self.getVolume(fp, level, return_shape=True)
error = (vol.Value - shape.Volume) / fp.Shape.Volume
if abs(error) < 0.01:
break
level += error
# Get the center of gravity
vol = 0.0
cog = Vector()
if len(shape.Solids) > 0:
for solid in shape.Solids:
vol += solid.Volume
sCoG = solid.CenterOfMass
cog.x = cog.x + sCoG.x * solid.Volume
cog.y = cog.y + sCoG.y * solid.Volume
cog.z = cog.z + sCoG.z * solid.Volume
cog.x = cog.x / vol
cog.y = cog.y / vol
cog.z = cog.z / vol
# Untransform the object to retrieve the original position
fp.Placement = current_placement
p = Part.Point(cog)
m = Matrix()
m.rotateY(trim.getValueAs("rad"))
m.rotateX(-roll.getValueAs("rad"))
p.rotate(Placement(m))
return Vector(p.X, p.Y, p.Z)
def displacement(ship, draft=None,
roll=Units.parseQuantity("0 deg"),
trim=Units.parseQuantity("0 deg")):
"""Compute the ship displacement
Position arguments:
ship -- Ship object (see createShip)
Keyword arguments:
draft -- Ship draft (Design ship draft by default)
roll -- Roll angle (0 degrees by default)
trim -- Trim angle (0 degrees by default)
Returned values:
disp -- The ship displacement (a density of the water of 1025 kg/m^3 is
assumed)
B -- Bouyance application point, i.e. Center of mass of the underwater side
Cb -- Block coefficient
The Bouyance center is refered to the original ship position.
"""
if draft is None:
draft = ship.Draft
shape, base_z = placeShipShape(ship.Shape.copy(), draft, roll, trim)
shape = getUnderwaterSide(shape)
vol = 0.0
cog = Vector()
if len(shape.Solids) > 0:
for solid in shape.Solids:
vol += solid.Volume
sCoG = solid.CenterOfMass
cog.x = cog.x + sCoG.x * solid.Volume
cog.y = cog.y + sCoG.y * solid.Volume
cog.z = cog.z + sCoG.z * solid.Volume
cog.x = cog.x / vol
cog.y = cog.y / vol
cog.z = cog.z / vol
bbox = shape.BoundBox
Vol = (bbox.XMax - bbox.XMin) * (bbox.YMax - bbox.YMin) * abs(bbox.ZMin)
# Undo the transformations on the bouyance point
B = Part.Point(Vector(cog.x, cog.y, cog.z))
m = Matrix()
m.move(Vector(0.0, 0.0, draft))
m.move(Vector(-draft * math.sin(trim.getValueAs("rad")), 0.0, 0.0))
m.rotateY(trim.getValueAs("rad"))
m.move(Vector(0.0,
-draft * math.sin(roll.getValueAs("rad")),
base_z))
m.rotateX(-roll.getValueAs("rad"))
B.transform(m)
try:
cb = vol / Vol
except ZeroDivisionError:
msg = QtGui.QApplication.translate(
"ship_console",
"ZeroDivisionError: Null volume found during the displacement"
" computation!",
None,
QtGui.QApplication.UnicodeUTF8)
App.Console.PrintError(msg + '\n')
cb = 0.0
# Return the computed data
return (DENS * Units.Quantity(vol, Units.Volume),
Vector(B.X, B.Y, B.Z),
cb)
def displacement(ship,
draft=None,
roll=Units.parseQuantity("0 deg"),
trim=Units.parseQuantity("0 deg")):
"""Compute the ship displacement
Position arguments:
ship -- Ship object (see createShip)
Keyword arguments:
draft -- Ship draft (Design ship draft by default)
roll -- Roll angle (0 degrees by default)
trim -- Trim angle (0 degrees by default)
Returned values:
disp -- The ship displacement (a density of the water of 1025 kg/m^3 is
assumed)
B -- Bouyance application point, i.e. Center of mass of the underwater side
Cb -- Block coefficient
The Bouyance center is referred to the original ship position.
"""
if draft is None:
draft = ship.Draft
shape, base_z = placeShipShape(ship.Shape.copy(), draft, roll, trim)
shape = getUnderwaterSide(shape)
vol = 0.0
cog = Vector()
if len(shape.Solids) > 0:
for solid in shape.Solids:
vol += solid.Volume
sCoG = solid.CenterOfMass
cog.x = cog.x + sCoG.x * solid.Volume
cog.y = cog.y + sCoG.y * solid.Volume
cog.z = cog.z + sCoG.z * solid.Volume
cog.x = cog.x / vol
cog.y = cog.y / vol
cog.z = cog.z / vol
bbox = shape.BoundBox
Vol = (bbox.XMax - bbox.XMin) * (bbox.YMax - bbox.YMin) * abs(bbox.ZMin)
# Undo the transformations on the bouyance point
B = Part.Point(Vector(cog.x, cog.y, cog.z))
m = Matrix()
m.move(Vector(0.0, 0.0, draft))
m.move(Vector(-draft * math.sin(trim.getValueAs("rad")), 0.0, 0.0))
m.rotateY(trim.getValueAs("rad"))
m.move(Vector(0.0, -draft * math.sin(roll.getValueAs("rad")), base_z))
m.rotateX(-roll.getValueAs("rad"))
B.transform(m)
try:
cb = vol / Vol
except ZeroDivisionError:
msg = QtGui.QApplication.translate(
"ship_console",
"ZeroDivisionError: Null volume found during the displacement"
" computation!", None)
App.Console.PrintError(msg + '\n')
cb = 0.0
# Return the computed data
return (DENS * Units.Quantity(vol, Units.Volume), Vector(B.X, B.Y,
B.Z), cb)
def rotate(u, angle, axis=Vector(0, 0, 1)):
"""Rotate the vector by the specified angle, around the given axis.
If the axis is omitted, the rotation is made around the Z axis
(on the XY plane).
It uses a 3x3 rotation matrix.
::
u_rot = R u
(c + x*x*t xyt - zs xzt + ys )
u_rot = (xyt + zs c + y*y*t yzt - xs ) * u
(xzt - ys yzt + xs c + z*z*t)
Where `x`, `y`, `z` indicate unit components of the axis;
`c` denotes a cosine of the angle; `t` indicates a complement
of that cosine; `xs`, `ys`, `zs` indicate products of the unit
components and the sine of the angle; and `xyt`, `xzt`, `yzt`
indicate products of two unit components and the complement
of the cosine.
Parameters
----------
u : Base::Vector3
The vector.
angle : float
The angle of rotation given in radians.
axis : Base::Vector3, optional
The vector specifying the axis of rotation.
It defaults to `(0, 0, 1)`, the +Z axis.
Returns
-------
Base::Vector3
The new rotated vector.
If the `angle` is zero, return the original vector `u`.
"""
typecheck([(u, Vector), (angle, (int, float)), (axis, Vector)], "rotate")
if angle == 0:
return u
# Unit components, so that x**2 + y**2 + z**2 = 1
L = axis.Length
x = axis.x / L
y = axis.y / L
z = axis.z / L
c = math.cos(angle)
s = math.sin(angle)
t = 1 - c
# Various products
xyt = x * y * t
xzt = x * z * t
yzt = y * z * t
xs = x * s
ys = y * s
zs = z * s
m = Matrix(c + x * x * t, xyt - zs, xzt + ys, 0, xyt + zs, c + y * y * t,
yzt - xs, 0, xzt - ys, yzt + xs, c + z * z * t, 0)
return m.multiply(u)
def cone_setup(doc, profil, data):
"""create all the shapes needed for the cone parts"""
# if the final shapes already existed (true if one shape exists)
# return them immediatly
cone_top_obj = doc.getObject('_cone_top_base')
cone_side_obj = doc.getObject('_cone_side_base')
cone_struct_obj = doc.getObject('_cone_struct_base')
cone_top_thread_obj = doc.getObject('_cone_top_thread')
cone_struct_thread_obj = doc.getObject('_cone_struct_thread')
if cone_top_obj:
cone_top = cone_top_obj.Shape.copy()
cone_side0 = cone_side_obj.Shape.copy()
cone_side1 = cone_side_obj.Shape.copy()
cone_side2 = cone_side_obj.Shape.copy()
cone_struct = cone_struct_obj.Shape.copy()
cone_top_thread = cone_top_thread_obj.Shape.copy()
cone_struct_thread = cone_struct_thread_obj.Shape.copy()
return cone_top, cone_side0, cone_side1, cone_side2, cone_struct, \
cone_top_thread, cone_struct_thread
side = profil['side']
radius = profil['radius']
diam_int = data['diameter']
diam_ext = data['diameter'] + data['thick']
length = data['len_lo'] + data['len_hi']
struct_thick = data['struct_thick']
# to modify the sphere to make it a ellipsoid
matrix = Matrix()
matrix.scale(1., 1., length / (diam_ext / 2))
# to suppress the lower half of the sphere/ellipsoid
lower = Part.makeBox(diam_ext, diam_ext, length)
lower.translate(Vector(-diam_ext / 2, -diam_ext / 2, 0))
gui_doc = FreeCADGui.ActiveDocument
# make the external shape base of the cone
sphere = Part.makeSphere(diam_ext / 2)
sphere = sphere.transformGeometry(matrix)
cone_base_ext = sphere.common(lower)
cone_base_ext_obj = doc.addObject("Part::Feature", '_cone_base_ext')
cone_base_ext_obj.Shape = cone_base_ext
gui_doc.getObject('_cone_base_ext').Visibility = False
# make the internal shape base of the cone
sphere = Part.makeSphere(diam_int / 2)
sphere = sphere.transformGeometry(matrix)
cone_base_int = sphere.common(lower)
cone_base_int_obj = doc.addObject("Part::Feature", '_cone_base_int')
cone_base_int_obj.Shape = cone_base_int
gui_doc.getObject('_cone_base_int').Visibility = False
# make the skin
skin = cone_base_ext.cut(cone_base_int)
skin_obj = doc.addObject("Part::Feature", '_skin')
skin_obj.Shape = skin
gui_doc.getObject('_skin').Visibility = False
# use profile shapes to make suppressed parts of the skin
# full profil part
shape = []
shape.append(Vector(radius - 10, side / 2, 0))
shape.append(Vector(radius + diam_ext, side / 2, 0))
shape.append(Vector(radius + diam_ext, -side / 2, 0))
shape.append(Vector(radius - 10, -side / 2, 0))
shape.append(Vector(radius - 10, side / 2, 0))
wire = Part.makePolygon(shape)
face = Part.Face(wire)
# make the volume
long_cut_base = face.extrude(Vector(0, 0, length))
long_cut_obj = doc.addObject("Part::Feature", '_long_cut_base')
long_cut_obj.Shape = long_cut_base
gui_doc.getObject('_long_cut_base').Visibility = False
# full profil part
shape = []
shape.append(Vector(radius, side / 2, 0))
shape.append(Vector(radius + diam_ext, side / 2, 0))
shape.append(Vector(radius + diam_ext, -side / 2, 0))
shape.append(Vector(radius, -side / 2, 0))
shape.append(Vector(radius, side / 2, 0))
wire = Part.makePolygon(shape)
face = Part.Face(wire)
# make the volume
short_cut_base = face.extrude(Vector(0, 0, data['len_lo']))
short_cut_obj = doc.addObject("Part::Feature", '_short_cut_base')
short_cut_obj.Shape = short_cut_base
gui_doc.getObject('_short_cut_base').Visibility = False
# create 1/3 cylinder
cylinder = Part.makeCylinder(diam_ext / 2, length, Vector(0, 0, 0), Vector(0, 0, 1), 120)
cylinder_obj = doc.addObject("Part::Feature", '_cylinder_1_3')
cylinder_obj.Shape = cylinder
gui_doc.getObject('_cylinder_1_3').Visibility = False
# thread bases
radius_ext = diam_ext / 2 - struct_thick - 5
thread_ext = Part.makeHelix(8, struct_thick - 8, radius_ext)
radius_int = diam_ext / 2 - struct_thick - 9
thread_int = Part.makeHelix(8, struct_thick, radius_int)
# tube to make the space for threads
tube_thread_ext = Part.makeCylinder(radius_ext, struct_thick)
tube_thread_int = Part.makeCylinder(radius_int, struct_thick)
tube_thread = tube_thread_ext.cut(tube_thread_int)
tube_thread_obj = doc.addObject("Part::Feature", '_tube_thread')
tube_thread_obj.Shape = tube_thread
gui_doc.getObject('_tube_thread').Visibility = False
# tube to cut the top of the structure
tube_cut_ext = Part.makeCylinder(diam_ext / 2 - struct_thick / 2, struct_thick)
tube_cut_int = tube_thread_int
tube_cut = tube_cut_ext.cut(tube_cut_int)
tube_cut_obj = doc.addObject("Part::Feature", '_tube_cut')
tube_cut_obj.Shape = tube_cut
gui_doc.getObject('_tube_cut').Visibility = False
# tube to make the locking of the cone
tube_lock_0 = Part.makeCylinder(diam_ext / 2 - struct_thick - 2, struct_thick / 2)
tube_lock_1 = Part.makeCylinder(diam_ext / 2 - struct_thick + 2, struct_thick / 2)
tube_lock_2 = Part.makeCylinder(diam_ext / 2, struct_thick / 4)
tube_lock_int = tube_lock_1.cut(tube_lock_0)
tube_lock_int.translate(Vector(0, 0, data['len_lo'] - 0.25 * data['struct_thick']))
tube_lock_obj = doc.addObject("Part::Feature", '_tube_lock_int')
tube_lock_obj.Shape = tube_lock_int
gui_doc.getObject('_tube_lock_int').Visibility = False
tube_lock_ext = tube_lock_2.cut(tube_lock_0)
tube_lock_ext.translate(Vector(0, 0, data['len_lo'] - 0.25 * data['struct_thick']))
tube_lock_obj = doc.addObject("Part::Feature", '_tube_lock_ext')
tube_lock_obj.Shape = tube_lock_ext
gui_doc.getObject('_tube_lock_ext').Visibility = False
# make cone top part
cone_top = cone_top_make(doc, gui_doc, data, skin, lower, cone_base_int, tube_cut, tube_thread, tube_lock_int)
cone_top = cone_top.copy()
cone_side = cone_side_make(doc, gui_doc, skin, lower, data, cone_base_int, long_cut_base, cylinder, tube_lock_int, tube_lock_ext)
cone_side0 = cone_side.copy()
cone_side0.rotate(Vector(0, 0, 0), Vector(0, 0, 1), 0)
cone_side1 = cone_side.copy()
cone_side1.rotate(Vector(0, 0, 0), Vector(0, 0, 1), 120)
cone_side2 = cone_side.copy()
cone_side2.rotate(Vector(0, 0, 0), Vector(0, 0, 1), 240)
cone_struct = cone_struct_make(doc, gui_doc, cone_base_ext, short_cut_base, data, cone_top, cone_side, tube_thread)
# internal thread profile
p0 = (radius_int, 0, 0)
p1 = (radius_int + 4, 0, 4)
p2 = (radius_int, 0, 8)
e0 = Part.makeLine(p0, p1)
e1 = Part.makeLine(p1, p2)
e2 = Part.makeLine(p2, p0)
section = Part.Wire([e0, e1, e2])
cone_struct_thread = Part.Wire(thread_int).makePipeShell([section], 1, 1)
cone_struct_thread_obj = doc.addObject("Part::Feature", '_cone_struct_thread')
cone_struct_thread_obj.Shape = cone_struct_thread.copy()
gui_doc.getObject('_cone_struct_thread').Visibility = False
# external thread profile
p0 = (radius_ext, 0, 0)
p1 = (radius_ext - 4, 0, 4)
p2 = (radius_ext, 0, 8)
e0 = Part.makeLine(p0, p1)
e1 = Part.makeLine(p1, p2)
e2 = Part.makeLine(p2, p0)
section = Part.Wire([e0, e1, e2])
cone_top_thread = Part.Wire(thread_ext).makePipeShell([section], 1, 1)
cone_top_thread_obj = doc.addObject("Part::Feature", '_cone_top_thread')
cone_top_thread_obj.Shape = cone_top_thread.copy()
gui_doc.getObject('_cone_top_thread').Visibility = False
return cone_top, cone_side0, cone_side1, cone_side2, cone_struct, \
cone_top_thread, cone_struct_thread
def getCoG(self, fp, vol, roll=Units.parseQuantity("0 deg"),
trim=Units.parseQuantity("0 deg")):
"""Return the fluid volume center of gravity, provided the volume of
fluid inside the tank.
The returned center of gravity is referred to the untransformed ship.
Keyword arguments:
fp -- Part::FeaturePython object affected.
vol -- Volume of fluid.
roll -- Ship roll angle.
trim -- Ship trim angle.
If the fluid volume is bigger than the total tank one, it will be
conveniently clamped.
"""
# Change the units of the volume, and clamp the value
if vol <= 0.0:
return Vector()
if vol >= fp.Shape.Volume:
vol = 0.0
for solid in fp.Shape.Solids:
vol += solid.Volume
sCoG = solid.CenterOfMass
cog.x = cog.x + sCoG.x * solid.Volume
cog.y = cog.y + sCoG.y * solid.Volume
cog.z = cog.z + sCoG.z * solid.Volume
cog.x = cog.x / vol
cog.y = cog.y / vol
cog.z = cog.z / vol
return cog
# Get a first estimation of the level
level = vol.Value / fp.Shape.Volume
# Transform the tank shape
current_placement = fp.Placement
m = current_placement.toMatrix()
m.rotateX(roll.getValueAs("rad"))
m.rotateY(-trim.getValueAs("rad"))
fp.Placement = Placement(m)
# Iterate to find the fluid shape
for i in range(COMMON_BOOLEAN_ITERATIONS):
shape = self.getVolume(fp, level, return_shape=True)
error = (vol.Value - shape.Volume) / fp.Shape.Volume
if abs(error) < 0.01:
break
level += error
# Get the center of gravity
vol = 0.0
cog = Vector()
if len(shape.Solids) > 0:
for solid in shape.Solids:
vol += solid.Volume
sCoG = solid.CenterOfMass
cog.x = cog.x + sCoG.x * solid.Volume
cog.y = cog.y + sCoG.y * solid.Volume
cog.z = cog.z + sCoG.z * solid.Volume
cog.x = cog.x / vol
cog.y = cog.y / vol
cog.z = cog.z / vol
# Untransform the object to retrieve the original position
fp.Placement = current_placement
p = Part.Point(cog)
m = Matrix()
m.rotateY(trim.getValueAs("rad"))
m.rotateX(-roll.getValueAs("rad"))
p.rotate(Placement(m))
return Vector(p.X, p.Y, p.Z)
