Added a much more robust displacement tool for the GZ computation

src/Mod/Ship/shipGZ/TaskPanel.py

@@ -57,7 +57,7 @@ class TaskPanel:
 
         gz = Tools.solve(self.ship,
                          self.weights,
-                         self.tanks
+                         self.tanks,
                          rolls,
                          form.var_trim.isChecked())
 

src/Mod/Ship/shipGZ/Tools.py

@@ -36,6 +36,7 @@ from shipHydrostatics import Tools as Hydrostatics
 G = 9.81
 MAX_EQUILIBRIUM_ITERS = 10
 DENS = 1.025  # [tons/m3], salt water
+TRIM_RELAX_FACTOR = 10.0
 
 
 def solve(ship, weights, tanks, rolls, var_trim=True):
@@ -68,14 +69,15 @@ def solve(ship, weights, tanks, rolls, var_trim=True):
         # t[2] = filling level
         vol = t[0].Proxy.setFillingLevel(t[0], t[2]).getValueAs('m^3').Value
         TW += vol * t[1]
-    TW = TW.getValueAs('kg').Value * G
+    TW = TW * G
 
     gzs = []
-    for roll in rolls:
+    for i,roll in enumerate(rolls):
+        App.Console.PrintMessage("{0} / {1}\n".format(i + 1, len(rolls)))
         gz = solve_point(W, COG, TW, ship, tanks, roll, var_trim)
         if gz is None:
             return []
-        gzs.append(solve_point(W, COG, TW, ship, tanks, roll, var_trim))
+        gzs.append(gz)
 
     return gzs
 
@@ -92,9 +94,9 @@ def solve_point(W, COG, TW, ship, tanks, roll, var_trim=True):
     gz = 0.0
     
     # Look for the equilibrium draft (and eventually the trim angle too)
-    max_draft = ship.Shape.BoundBox.ZMax
+    max_draft = ship.Shape.BoundBox.ZMax / Units.Metre.Value
-    draft = max_draft
+    draft = ship.Draft.getValueAs('m').Value
-    max_disp = ship.Shape.Volume.getValueAs('m^3').Value * DENS * 1000.0 * G
+    max_disp = ship.Shape.Volume / Units.Metre.Value**3 * DENS * 1000.0 * G
     if max_disp < W + TW:
         msg = QtGui.QApplication.translate(
             "ship_console",
@@ -109,35 +111,32 @@ def solve_point(W, COG, TW, ship, tanks, roll, var_trim=True):
         # Get the displacement, and the bouyance application point
         disp, B, Cb = Hydrostatics.displacement(ship, draft, roll, trim)
         disp *= 1000.0 * G
-        # Get the empty ship weight transformed application point
+        # Add the tanks effect on the center of gravity
-        p = Part.makePoint(COG)
-        p.translate(Vector(0.0, 0.0, -draft))
-        m = Matrix()
-        m.rotateX(math.radians(roll))
-        m.rotateY(-math.radians(trim))
-        p.rotate(Placement(m))
-        # Add the tanks
         # TODO
         # ---
 
         # Compute the errors
-        draft_error = abs(disp - W - TW) / max_disp
+        draft_error = -(disp - W - TW) / max_disp
+        R = COG - B
         if not var_trim:
             trim_error = 0.0
         else:
-            dx = B.x - p.X 
+            trim_error = -TRIM_RELAX_FACTOR * R.x / ship.Length.getValueAs('m').Value
-            dz = B.z - p.Z
-            if abs(dx) < 0.001 * ship.Length.getValueAs('m').Value:
-                trim_error = 0.0
-            else:
-                trim_error = math.degrees(math.atan2(dz, dx))
 
         # Check if we can tolerate the errors
-        if draft_error < 0.01 and trim_error < 1.0:
+        if abs(draft_error) < 0.01 and abs(trim_error) < 0.05:
             break
 
         # Get the new draft and trim
         draft += draft_error * max_draft
-        trim += 0.5 * trim_error
+        trim += trim_error
 
-    return B.y - p.Y
+    App.Console.PrintMessage("draft and trim: {}, {}\n".format(draft, trim))
+
+    App.Console.PrintMessage(R)
+    App.Console.PrintMessage("\n")
+    c = math.cos(math.radians(roll))
+    s = math.sin(math.radians(roll))
+    App.Console.PrintMessage(c * R.y - s * R.z)
+    App.Console.PrintMessage("\n")
+    return c * R.y - s * R.z

src/Mod/Ship/shipHydrostatics/Tools.py

@@ -22,13 +22,20 @@
 #***************************************************************************
 
 import math
-from FreeCAD import Vector, Matrix, Placement
+import random
+from FreeCAD import Vector, Rotation, Matrix, Placement
 import Part
 import Units
 import FreeCAD as App
 import FreeCADGui as Gui
+from PySide import QtGui, QtCore
 import Instance
 from shipUtils import Math
+import shipUtils.Units as USys
+
+
+DENS = 1.025  # [tons/m3], salt water
+COMMON_BOOLEAN_ITERATIONS = 10
 
 
 def areas(ship, draft, roll=0.0, trim=0.0, yaw=0.0, n=30):
@@ -148,21 +155,41 @@ def displacement(ship, draft, roll=0.0, trim=0.0, yaw=0.0):
     L = xmax - xmin
     B = ymax - ymin
     H = zmax - zmin
-    p = Vector(xmin - L, ymin - B, zmin - H)
-    try:
-        box = Part.makeBox(3.0 * L, 3.0 * B, - zmin + H, p)
-    except Part.OCCError:
-        return [0.0, Vector(), 0.0]
-    Part.show(box)
-    box_shape = App.ActiveDocument.Objects[-1]
 
+    box = App.ActiveDocument.addObject("Part::Box","Box")
+    length_format = USys.getLengthFormat()
+    box.Placement = Placement(Vector(xmin - L, ymin - B, zmin - H),
+                              Rotation(App.Vector(0,0,1),0))
+    box.Length = length_format.format(3.0 * L)
+    box.Width = length_format.format(3.0 * B)
+    box.Height = length_format.format(- zmin + H)
+
+    App.ActiveDocument.recompute()
     common = App.activeDocument().addObject("Part::MultiCommon",
                                             "DisplacementHelper")
-    common.Shapes = [ship_shape, box_shape]
+    common.Shapes = [ship_shape, box]
+    App.ActiveDocument.recompute()
+    if len(common.Shape.Solids) == 0:
+        # The common operation is failing, let's try moving a bit the free
+        # surface
+        msg = QtGui.QApplication.translate(
+            "ship_console",
+            "Boolean operation failed. The tool is retrying that slightly"
+            " moving the free surface position",
+            None,
+            QtGui.QApplication.UnicodeUTF8)
+        App.Console.PrintWarning(msg + '\n')
+        random_bounds = 0.01 * H
+        i = 0
+        while len(common.Shape.Solids) == 0 and i < COMMON_BOOLEAN_ITERATIONS:
+            i += 1
+            box.Height = length_format.format(
+                - zmin + H + random.uniform(-random_bounds, random_bounds))
             App.ActiveDocument.recompute()
 
     vol = 0.0
     cog = Vector()
+    if len(common.Shape.Solids) > 0:
         for solid in common.Shape.Solids:
             vol += solid.Volume / Units.Metre.Value**3
             sCoG = solid.CenterOfMass
@@ -176,22 +203,24 @@ def displacement(ship, draft, roll=0.0, trim=0.0, yaw=0.0):
 
     App.ActiveDocument.removeObject(common.Name)
     App.ActiveDocument.removeObject(ship_shape.Name)
-    App.ActiveDocument.removeObject(box_shape.Name)
+    App.ActiveDocument.removeObject(box.Name)
+    App.ActiveDocument.recompute()
 
     # Undo the transformations
     B = Part.Point(Vector(cog.x, cog.y, cog.z))
     m = Matrix()
-    m.move(Vector(0.0, 0.0, _draft))
+    m.move(Vector(0.0, 0.0, draft))
     m.rotateZ(-math.radians(yaw))
-    m.move(Vector(-_draft * math.sin(math.radians(roll)), 0.0, 0.0))
+    m.move(Vector(-draft * math.sin(math.radians(roll)), 0.0, 0.0))
     m.rotateY(math.radians(trim))
-    m.move(Vector(0.0, -_draft * math.sin(math.radians(trim)), base_z))
+    m.move(Vector(0.0,
+                  -draft * math.sin(math.radians(trim)),
+                  base_z / Units.Metre.Value))
     m.rotateX(-math.radians(roll))
     B.transform(m)
 
     # Return the computed data
-    dens = 1.025  # [tons/m3], salt water
+    return [DENS*vol, Vector(B.X, B.Y, B.Z), vol/Vol]
-    return [dens*vol, Vector(B.X, B.Y, B.Z), vol/Vol]
 
 
 def wettedArea(shape, draft, trim):