Merge branch 'forces-torque' into main

Author: mfbehrens99

CHANGELOG.md

@@ -26,6 +26,10 @@ Template
 
 ### Added
 
+- [[Boat]] Add torques to the boat to make it spin
+- [[Simulation]] Run simulation for x steps
+- [[Sailor]] Basic rudder steering
+- [[GUI]] Display rudder forces in [boatInspector]
 - [[Boat]] Add setter methods for rudderAngle
 - [[GUI]] Display Sailor Waypoints on [mapView]
 - [[GUI]] Add goto start and goto end button
@@ -34,9 +38,18 @@ Template
 
 ### Changed
 
+- [[Boat]] Move forces and torques to their own files
+- [[Boat]] Rename waterDrag and waterLift to centerboardDrag and centerboardLift
+- [[Sailor]] Don't delete Waypoints after executing them
 - [[GUI]] Display mainSailAngle and rudderAngle in [boatInspector]
 - [[Sailor]] Use index instead of deleting [Commands] after executing them
 
+### Removed
+
+- [[Boat]] Removed Boat::getSpeedSq()
+
+
+
 ## [0.0.1] - 2021-04-13
 
 ### Added

sailsim/boat/Boat.py

@@ -1,6 +1,5 @@
-from math import sqrt, pi
+from math import sqrt, pi, sin, cos
 
-from sailsim.utils.constants import DENSITY_AIR, DENSITY_WATER
 from sailsim.utils.anglecalculations import angleKeepInterval, directionKeepInterval
 from sailsim.utils.coordconversion import cartToRadiusSq, cartToArg
 
@@ -11,42 +10,56 @@
 class Boat:
     """Holds all information about the boat and calculates its speed, forces and torques."""
 
-    from .boatgetset import getPos, getSpeed, setDirection, setDirectionDeg, setMainSailAngle, setMainSailAngleDeg, setRudderAngle, setRudderAngleDeg, setConstants
+    from .boatgetset import setBoat, getPos, getSpeed, setDirection, setDirectionDeg, setMainSailAngle, setMainSailAngleDeg, setRudderAngle, setRudderAngleDeg, setConstants
 
-    def __init__(self, posX=0, posY=0, direction=0, speedX=0, speedY=0):
+    def __init__(self, posX=0, posY=0, direction=0, speedX=0, speedY=0, angSpeed=0):
         """
         Create a boat.
 
         Args:
             posX:       x position of the boat (in m)
             posY:       y position of the boat (in m)
-            direction:  direction the boat is pointing
+            direction:  direction the boat is pointing (in rad)
             speedX:     speed in x direction (in m/s)
             speedY:     speed in y direction (in m/s)
+            angSpeed:   angular speed in z direction (in rad/s)
         """
+
+        # Static properties
+        self.length = 4.2               # m
+        self.width = 1.63               # m
+        self.mass = 80                  # kg
+        self.momentumInertia = 1/12 * self.mass * (pow(self.length, 2) + pow(self.width, 2))  # kg/m^2
+        self.sailArea = 7.45            # m^2
+        self.hullArea = 4               # m^2
+        self.centerboardArea = 1        # m^2 # self.centerboardDepth * self.centerboardLength
+        self.centerboardDepth = 0       # m
+        self.centerboardLength = 0      # m
+        self.centerboardLever = -0.3    # m
+        self.rudderArea = .175          # m^2 # self.rudderDepth * self.rudderLength
+        self.rudderDepth = 0            # m
+        self.rudderLength = 0           # m
+        self.rudderLever = 2.1          # m
+
         # Dynamic properties
         self.posX = posX
         self.posY = posY
-
         self.speedX = speedX
         self.speedY = speedY
-        self.direction = directionKeepInterval(direction)
 
-        self.rudderAngle = 0
-        self.maxRudderAngle = 80 / 180 * pi
+        self.direction = directionKeepInterval(direction)
+        self.angSpeed = angSpeed        # rad/s
+        self.pivot = 0.5 * self.length  # m
 
         self.mainSailAngle = 0
         self.maxMainSailAngle = 80 / 180 * pi
 
+        self.rudderAngle = 0
+        self.maxRudderAngle = 80 / 180 * pi
+
         self.dataHolder = BoatDataHolder()
         self.sailor = None
 
-        # Static properties
-        self.mass = 80
-        self.sailArea = 7.45
-        self.hullArea = 4 # arbitrary
-        self.centerboardArea = 1
-
         # Coefficients methods
         self.coefficientAirDrag = coefficientAirDrag
         self.coefficientAirLift = coefficientAirLift
@@ -57,23 +70,26 @@ def __init__(self, posX=0, posY=0, direction=0, speedX=0, speedY=0):
         self.tackingAngleDownwind = 20 / 180 * pi
 
 
-    # Simulation
-    def applyForce(self, forceX, forceY, interval):
-        """Change speed according a force given."""
-        # △v = a * t ; F = m * a
-        # △v = F / m * t
+    # Simulation methods
+    def applyCauses(self, forceX, forceY, torque, interval):
+        """Change speed according a force & torque given."""
+        # Translation: △v = a * t; F = m * a -> △v = F / m * t
+        # Rotation:    △ω = α * t; M = I * α -> △ω = M / I * t
         self.speedX += forceX / self.mass * interval
         self.speedY += forceY / self.mass * interval
+        self.angSpeed += torque / self.momentumInertia * interval
 
     def moveInterval(self, interval):
         """Change position according to sailsDirection and speed."""
-        # s = v * t
+        # △s = v * t; △α = ω * t
         self.posX += self.speedX * interval
         self.posY += self.speedY * interval
+        self.direction = directionKeepInterval(self.direction + self.angSpeed * interval)
 
     def runSailor(self):
         """Activate the sailing algorithm to decide what the boat should do."""
         if self.sailor is not None:
+            # Run sailor if sailor exists
             self.sailor.run(
                 self.posX,
                 self.posY,
@@ -82,83 +98,91 @@ def runSailor(self):
                 self.direction,
                 self.dataHolder.apparentWindSpeed,
                 self.dataHolder.apparentWindAngle
-            ) # Run sailor
+            )
 
-            # Set boat properties
+            # Retrieve boat properties from Sailor
             self.mainSailAngle = self.sailor.mainSailAngle
-            self.direction = self.sailor.boatDirection
-
+            # self.direction = self.sailor.boatDirection
+            self.rudderAngle = self.sailor.rudderAngle
 
-    # Force calculations
-    def resultingForce(self, trueWindX, trueWindY):
-        """Add up all reacting forces and return them as a tuple."""
+    def resultingCauses(self, trueWindX, trueWindY):
+        """Add up all acting forces and return them as a tuple."""
         h = self.dataHolder
 
+        h.boatSpeed = self.boatSpeed()
+
         # calculate apparent wind angle
         (h.apparentWindX, h.apparentWindY) = self.apparentWind(trueWindX, trueWindY)
         h.apparentWindAngle = self.apparentWindAngle(h.apparentWindX, h.apparentWindY)
-
         apparentWindSpeedSq = cartToRadiusSq(h.apparentWindX, h.apparentWindY)
         h.apparentWindSpeed = sqrt(apparentWindSpeedSq)
-        boatSpeedSq = self.boatSpeedSq()
-        h.boatSpeed = sqrt(boatSpeedSq)
+
+        # calculate flowSpeed
+        (flowSpeedRudderX, flowSpeedRudderY) = self.leverSpeedVector(self.rudderLever)
+        flowSpeedRudderSq = cartToRadiusSq(flowSpeedRudderX, flowSpeedRudderY)
+        flowSpeedRudder = sqrt(flowSpeedRudderSq)
+
+        (flowSpeedCenterboardX, flowSpeedCenterboardY) = self.leverSpeedVector(self.centerboardLever)
+        flowSpeedCenterboardSq = cartToRadiusSq(flowSpeedCenterboardX, flowSpeedCenterboardY)
+        flowSpeedCenterboard = sqrt(flowSpeedCenterboardSq)
 
         # normalise apparent wind vector and boat speed vetor
         # if vector is (0, 0) set normalised vector to (0, 0) aswell
         (apparentWindNormX, apparentWindNormY) = (h.apparentWindX / h.apparentWindSpeed, h.apparentWindY / h.apparentWindSpeed) if not h.apparentWindSpeed == 0 else (0, 0) # normalised apparent wind vector
-        (speedNormX, speedNormY) = (self.speedX / h.boatSpeed, self.speedY / h.boatSpeed) if not h.boatSpeed == 0 else (0, 0) # normalised speed vector
+        # (speedNormX, speedNormY) = (self.speedX / h.boatSpeed, self.speedY / h.boatSpeed) if not h.boatSpeed == 0 else (0, 0) # normalised speed vector
+        (flowSpeedRudderNormX, flowSpeedRudderNormY) = (flowSpeedRudderX / flowSpeedRudder, flowSpeedRudderY / flowSpeedRudder) if not flowSpeedRudder == 0 else (0, 0) # normalised speed vector
+        (flowSpeedCenterboardNormX, flowSpeedCenterboardNormY) = (flowSpeedCenterboardX / flowSpeedCenterboard, flowSpeedCenterboardY / flowSpeedCenterboard) if not flowSpeedCenterboard == 0 else (0, 0) # normalised speed vector
+        (dirNormX, dirNormY) = (sin(self.direction), cos(self.direction))
 
         h.leewayAngle = self.calcLeewayAngle()
         h.angleOfAttack = self.angleOfAttack(h.apparentWindAngle)
 
-        # Sum up all acting forces
-        (forceX, forceY) = (0, 0)
-        (h.sailDragX, h.sailDragY) = self.sailDrag(apparentWindNormX, apparentWindNormY, apparentWindSpeedSq)
+        (forceX, forceY, torque) = (0, 0, 0)
+
+        # Sail forces
+        (h.sailDragX, h.sailDragY) = self.sailDrag(apparentWindSpeedSq, apparentWindNormX, apparentWindNormY)
         forceX += h.sailDragX
         forceY += h.sailDragY
-        (h.sailLiftX, h.sailLiftY) = self.sailLift(apparentWindNormX, apparentWindNormY, apparentWindSpeedSq)
+        (h.sailLiftX, h.sailLiftY) = self.sailLift(apparentWindSpeedSq, apparentWindNormX, apparentWindNormY)
         forceX += h.sailLiftX
         forceY += h.sailLiftY
 
-        (h.waterDragX, h.waterDragY) = self.waterDrag(speedNormX, speedNormY, boatSpeedSq)
-        forceX += h.waterDragX
-        forceY += h.waterDragY
-        (h.waterLiftX, h.waterLiftY) = self.waterLift(speedNormX, speedNormY, boatSpeedSq)
-        forceX += h.waterLiftX
-        forceY += h.waterLiftY
-
-        (h.forceX, h.forceY) = (forceX, forceY)
-        return (forceX, forceY)
-
-    def sailDrag(self, apparentWindNormX, apparentWindNormY, apparentWindSpeedSq):
-        """Calculate the force that is created when wind blows against the boat."""
-        force = 0.5 * DENSITY_AIR * self.sailArea * apparentWindSpeedSq * self.coefficientAirDrag(self.dataHolder.angleOfAttack)
-        return (force * apparentWindNormX, force * apparentWindNormY)
-
-    def sailLift(self, apparentWindNormX, apparentWindNormY, apparentWindSpeedSq):
-        """Calculate the lift force that is created when the wind changes its direction in the sail."""
-        force = 0.5 * DENSITY_AIR * self.sailArea * apparentWindSpeedSq * self.coefficientAirLift(self.dataHolder.angleOfAttack)
-        if self.dataHolder.angleOfAttack < 0:
-            return (-force * apparentWindNormY, force * apparentWindNormX)  # rotate by 90° counterclockwise
-        return (force * apparentWindNormY, -force * apparentWindNormX)      # rotate by 90° clockwise
-
-    def waterDrag(self, speedNormX, speedNormY, boatSpeedSq):
-        """Calculate the drag force of the water that is decelerating the boat."""
-        force = -0.5 * DENSITY_WATER * (self.hullArea + self.centerboardArea) * boatSpeedSq * self.coefficientWaterDrag(self.dataHolder.leewayAngle)
-        return (force * speedNormX, force * speedNormY)
-
-    def waterLift(self, speedNormX, speedNormY, boatSpeedSq):
-        """Calculate force that is caused by lift forces in the water."""
-        force = -0.5 * DENSITY_WATER * self.centerboardArea * boatSpeedSq * self.coefficientWaterLift(self.dataHolder.leewayAngle)
-        if self.dataHolder.leewayAngle < 0:
-            return (-force * speedNormY, force * speedNormX)    # rotate by 90° counterclockwise
-        return (force * speedNormY, -force * speedNormX)        # rotate by 90° clockwise
-
-
-    # Speed calculations
-    def boatSpeedSq(self):
-        """Return speed of the boat but squared."""
-        return pow(self.speedX, 2) + pow(self.speedY, 2)
+        # Centerboard forces
+        (h.centerboardDragX, h.centerboardDragY) = self.centerboardDrag(flowSpeedCenterboardSq, flowSpeedCenterboardNormX, flowSpeedCenterboardNormY)
+        forceX += h.centerboardDragX
+        forceY += h.centerboardDragY
+        (h.centerboardLiftX, h.centerboardLiftY) = self.centerboardLift(flowSpeedCenterboardSq, flowSpeedCenterboardNormX, flowSpeedCenterboardNormY)
+        forceX += h.centerboardLiftX
+        forceY += h.centerboardLiftY
+
+        # Rudder forces
+        (h.rudderDragX, h.rudderDragY) = self.rudderDrag(flowSpeedRudderSq, flowSpeedRudderNormX, flowSpeedRudderNormY)
+        forceX += h.rudderDragX
+        forceY += h.rudderDragY
+        (h.rudderLiftX, h.rudderLiftY) = self.rudderLift(flowSpeedRudderSq, flowSpeedRudderNormX, flowSpeedRudderNormY)
+        forceX += h.rudderLiftX
+        forceY += h.rudderLiftY
+
+        # TODO Hull forces
+
+
+        # Torques
+        h.waterDragTorque = self.waterDragTorque()
+        torque += h.waterDragTorque
+
+        h.centerboardTorque = self.centerboardTorque(h.centerboardDragX + h.centerboardLiftX, h.centerboardDragY + h.centerboardLiftY, dirNormX, dirNormY)
+        torque += h.centerboardTorque
+
+        h.rudderTorque = self.rudderTorque(h.rudderDragX + h.rudderLiftX, h.rudderDragY + h.rudderLiftY, dirNormX, dirNormY)
+        torque += h.rudderTorque
+
+        (h.forceX, h.forceY, h.torque) = (forceX, forceY, torque)
+        return (forceX, forceY, torque)
+
+    # Import force and torque functions
+    from .boat_forces import leverSpeedVector, sailDrag, sailLift, centerboardDrag, centerboardLift, rudderDrag, rudderLift, scalarToDragForce, scalarToLiftForce
+    from .boat_torques import waterDragTorque, centerboardTorque, rudderTorque
+
 
     def boatSpeed(self):
         """Return speed of the boat."""
@@ -185,4 +209,4 @@ def angleOfAttack(self, apparentWindAngle):
 
     def __repr__(self):
         heading = round(cartToArg(self.speedX, self.speedY) * 180 / pi, 2)
-        return "Boat @(%s|%s), v=%sm/s twds %s°" % (round(self.posX, 3), round(self.posY, 3), round(sqrt(self.boatSpeedSq()), 2), heading)
+        return "Boat @(%s|%s), v=%sm/s twds %s°" % (round(self.posX, 3), round(self.posY, 3), round(self.boatSpeed(), 2), heading)

sailsim/boat/BoatDataHolder.py

@@ -12,5 +12,11 @@ def __init__(self):
         self.forceX = self.forceY = None
         self.sailDragX = self.sailDragY = None
         self.sailLiftX = self.sailLiftY = None
-        self.waterDragX = self.waterDragY = None
-        self.waterLiftX = self.waterLiftY = None
+        self.centerboardDragX = self.centerboardDragY = None
+        self.centerboardLiftX = self.centerboardLiftY = None
+        self.rudderDragX = self.rudderDragY = None
+        self.rudderLiftX = self.rudderLiftY = None
+
+        self.torque = None
+        self.waterDragTorque = None
+        self.centerboardTorque = self.rudderTorque = None

sailsim/boat/boat_forces.py

@@ -0,0 +1,67 @@
+"""This module holdes some force calculation for the Boat class."""
+
+from math import pi
+
+from sailsim.utils.anglecalculations import angleKeepInterval, directionKeepInterval
+from sailsim.utils.coordconversion import polarToCart
+from sailsim.utils.constants import DENSITY_AIR, DENSITY_WATER
+
+
+def leverSpeedVector(self, lever):
+    """Calculate the speed vector at a certain point concidering the rotation."""
+    (orbSpeedX, orbSpeedY) = polarToCart(self.angSpeed * lever, directionKeepInterval(self.direction + pi))
+    return (-self.speedX + orbSpeedX, -self.speedY + orbSpeedY)
+
+
+# Sail forces
+def sailDrag(self, apparentWindSpeedSq, apparentWindNormX, apparentWindNormY):
+    """Calculate the force that is created when wind blows against the boat."""
+    scalarSailDrag = 0.5 * DENSITY_AIR * self.sailArea * apparentWindSpeedSq * self.coefficientAirDrag(self.dataHolder.angleOfAttack)
+    return self.scalarToDragForce(scalarSailDrag, apparentWindNormX, apparentWindNormY)
+
+
+def sailLift(self, apparentWindSpeedSq, apparentWindNormX, apparentWindNormY):
+    """Calculate the lift force that is created when the wind changes its direction in the sail."""
+    scalarSailLift = 0.5 * DENSITY_AIR * self.sailArea * apparentWindSpeedSq * self.coefficientAirLift(self.dataHolder.angleOfAttack)
+    return self.scalarToLiftForce(scalarSailLift, self.dataHolder.angleOfAttack, apparentWindNormX, apparentWindNormY)
+
+
+# Centerboard forces
+def centerboardDrag(self, flowSpeedSq, flowSpeedCenterboardNormX, flowSpeedCenterboardNormY):
+    """Calculate the drag force of the water that is decelerating the boat."""
+    scalarCenterboardDrag = 0.5 * DENSITY_WATER * self.centerboardArea * flowSpeedSq * self.coefficientWaterDrag(self.dataHolder.leewayAngle)
+    return self.scalarToDragForce(scalarCenterboardDrag, flowSpeedCenterboardNormX, flowSpeedCenterboardNormY)
+
+
+def centerboardLift(self, flowSpeedSq, flowSpeedCenterboardNormX, flowSpeedCenterboardNormY):
+    """Calculate force that is caused by lift forces in the water."""
+    scalarCenterboardLift = 0.5 * DENSITY_WATER * self.centerboardArea * flowSpeedSq * self.coefficientWaterLift(self.dataHolder.leewayAngle)
+    return self.scalarToLiftForce(scalarCenterboardLift, self.dataHolder.leewayAngle, flowSpeedCenterboardNormX, flowSpeedCenterboardNormY)
+
+
+# Rudder forces
+def rudderDrag(self, flowSpeedSq, flowSpeedRudderNormX, flowSpeedRudderNormY):
+    """Calculates Force of the rudder that ist decelerating the boat."""
+    scalarRudderDrag = 0.5 * DENSITY_WATER * self.rudderArea * flowSpeedSq * self.coefficientWaterDrag(angleKeepInterval(self.dataHolder.leewayAngle + self.rudderAngle))
+    return self.scalarToDragForce(scalarRudderDrag, flowSpeedRudderNormX, flowSpeedRudderNormY)
+
+
+def rudderLift(self, flowSpeedSq, flowSpeedRudderNormX, flowSpeedRudderNormY):
+    """Calculates Lift caused by rudder."""
+    scalarRudderLift = 0.5 * DENSITY_WATER * self.rudderArea * flowSpeedSq * self.coefficientWaterLift(angleKeepInterval(self.dataHolder.leewayAngle + self.rudderAngle))
+    return self.scalarToLiftForce(scalarRudderLift, angleKeepInterval(self.dataHolder.leewayAngle + self.rudderAngle), flowSpeedRudderNormX, flowSpeedRudderNormY)
+
+
+
+
+# Conversions
+def scalarToLiftForce(self, scalarForce, angleOfAttack, normX, normY):
+    """Convert scalar lift force into cartesian vector"""
+    if angleOfAttack < 0:
+        return (-scalarForce * normY, scalarForce * normX)    # rotate by 90° counterclockwise
+    return (scalarForce * normY, -scalarForce * normX)        # rotate by 90° clockwise
+
+
+def scalarToDragForce(self, scalarForce, normX, normY):
+    """Convert scalar drag force into cartesian vector"""
+    return (scalarForce * normX, scalarForce * normY)

sailsim/boat/boat_torques.py

@@ -0,0 +1,21 @@
+"""This module calculates torques for the boat class."""
+
+from sailsim.utils.constants import DENSITY_AIR, DENSITY_WATER
+
+
+def waterDragTorque(self):
+    c_w = 1.1
+    # TODO use c_w
+    # NOTE formula does not contain an area (but only a length)
+    # print(self.angSpeed)
+    torque = 1 / 4 * c_w * DENSITY_WATER * pow(self.length, 4) * pow(self.angSpeed, 2)
+    if self.angSpeed < 0:
+        return torque
+    return -torque
+
+
+def centerboardTorque(self, centerboardX, centerboardY, dirNormX, dirNormY):
+    return (centerboardY * dirNormX - centerboardX * dirNormY) * self.centerboardLever # negative cross product
+
+def rudderTorque(self, rudderX, rudderY, dirNormX, dirNormY):
+    return (rudderY * dirNormX - rudderX * dirNormY) * self.rudderLever # negative cross product