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 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 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 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 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)
