Fixed edge cases.

UBCSailbot · Mar 2, 2024 · d571917 · d571917
1 parent 1d85b5c
commit d571917
Show file tree

Hide file tree

Showing 3 changed files with 174 additions and 99 deletions.
diff --git a/boat_simulator/nodes/physics_engine/fluid_forces.py b/boat_simulator/nodes/physics_engine/fluid_forces.py
@@ -1,12 +1,13 @@
 """This module provides functionality for computing the lift and drag forces acting on a medium."""
 
-from typing import Tuple
+from typing import Tuple, Union
 
+# from typing import Scalar
+import matplotlib.patches as patches
 import matplotlib.pyplot as plt
 import numpy as np
 from numpy.typing import NDArray
-import matplotlib.patches as patches
-from typing import TypeVar, Union
+
 Scalar = Union[int, float]
 
 # from boat_simulator.common.utils import Scalar
@@ -53,15 +54,16 @@ def calculate_attack_angle(self, apparent_velocity: NDArray, orientation: Scalar
 
         Returns:
             Scalar: The angle of attack formed between the orientation angle of the medium and
-                the direction of the apparent velocity, expressed in degrees and bounded between 0 and 180 degrees.
+                the direction of the apparent velocity, expressed in degrees
+                and bounded between 0 and 180 degrees.
         """
         # Calculate the angle in degrees and normalize it to the range [0, 360)
         angle_of_attack = (
             np.rad2deg(
                 np.arctan2(apparent_velocity[1], apparent_velocity[0]) - np.deg2rad(orientation)
             )
         ) % 360
-        
+
         if angle_of_attack > 180:
             angle_of_attack = 360 - angle_of_attack
 
@@ -86,26 +88,47 @@ def compute(self, apparent_velocity: NDArray, orientation: Scalar) -> Tuple[NDAr
         attack_angle = self.calculate_attack_angle(apparent_velocity, orientation)
         lift_coefficient, drag_coefficient, area = self.interpolate(attack_angle)
         velocity_magnitude = np.linalg.norm(apparent_velocity)
+
+        # Calculate the lift and drag forces
         lift_force_magnitude = (
-            0.5
-            * self.__fluid_density
-            * lift_coefficient
-            * area
-            * (velocity_magnitude ** 2)
+            0.5 * self.__fluid_density * lift_coefficient * area * (velocity_magnitude**2)
         )
 
         drag_force_magnitude = (
-            0.5
-            * self.__fluid_density
-            * drag_coefficient
-            * area
-            * (velocity_magnitude ** 2)
+            0.5 * self.__fluid_density * drag_coefficient * area * (velocity_magnitude**2)
         )
 
         # Rotate the lift and drag forces by 90 degrees to obtain the lift and drag forces
         # Rotate counter clockwise in 1st and 3rd quadrant, and clockwise in 2nd and 4th quadrant
         # 0 otherwise
         drag_force_direction = (apparent_velocity) / velocity_magnitude
+
+        def rotate_vector_clockwise(v, theta_degrees):
+            theta_radians = np.deg2rad(theta_degrees)  # Convert angle from degrees to radians
+            rotation_matrix = np.array(
+                [
+                    [np.cos(theta_radians), np.sin(theta_radians)],
+                    [-np.sin(theta_radians), np.cos(theta_radians)],
+                ]
+            )
+            v_rotated = np.dot(rotation_matrix, v)  # Multiply the rotation matrix by the vector
+            return v_rotated
+
+        def rotate_vector_anticlockwise(v, theta_degrees):
+            theta_radians = np.deg2rad(theta_degrees)  # Convert angle from degrees to radians
+            rotation_matrix = np.array(
+                [
+                    [np.cos(theta_radians), -np.sin(theta_radians)],
+                    [np.sin(theta_radians), np.cos(theta_radians)],
+                ]
+            )
+            v_rotated = np.dot(rotation_matrix, v)  # Multiply the rotation matrix by the vector
+            return v_rotated
+
+        # Rotate the drag force direction to normalize it to 0 degrees
+        if orientation != 0:
+            drag_force_direction = rotate_vector_clockwise(drag_force_direction, orientation)
+
         if (drag_force_direction[0] > 0 and drag_force_direction[1] > 0) or (
             drag_force_direction[0] < 0 and drag_force_direction[1] < 0
         ):
@@ -117,10 +140,14 @@ def compute(self, apparent_velocity: NDArray, orientation: Scalar) -> Tuple[NDAr
         else:
             lift_force_direction = np.array([0, 0])
 
+        # Rotate the lift and drag forces back to the original orientation
+        if orientation != 0:
+            lift_force_direction = rotate_vector_anticlockwise(lift_force_direction, orientation)
+            drag_force_direction = rotate_vector_anticlockwise(drag_force_direction, orientation)
+
         lift_force = lift_force_magnitude * lift_force_direction
         drag_force = drag_force_magnitude * drag_force_direction
 
-        # self.visualize_forces(apparent_velocity, lift_force, drag_force, orientation)
         return lift_force, drag_force
 
     def interpolate(self, attack_angle: Scalar) -> Tuple[Scalar, Scalar, Scalar]:
@@ -149,35 +176,58 @@ def interpolate(self, attack_angle: Scalar) -> Tuple[Scalar, Scalar, Scalar]:
         return lift_coefficient, drag_coefficient, area
 
     def draw_boat(ax, position, orientation):
-        """Draws a simplified boat shape on the given axes."""
+        """Draws a simplified boat shape on the given axes, ensuring it
+        aligns with the orientation line."""
         boat_length = 1.0
         boat_width = 0.3
 
-        # Create a simple boat shape
+        # Center the shape around (0, 0)
+        front_extension = 3 * boat_length / 4
+        rear_extension = -boat_length / 4
+
+        # Calculate the offset to center the shape
+        offset = front_extension + rear_extension
+
         boat_shape = np.array(
             [
-                [-boat_length / 2, -boat_width / 2],
-                [boat_length / 2, 0],
-                [-boat_length / 2, boat_width / 2],
-                [-boat_length / 2, -boat_width / 2],
+                [front_extension - offset, 0],
+                [boat_length / 2 - offset, boat_width / 2],
+                [rear_extension - offset, 0],
+                [boat_length / 2 - offset, -boat_width / 2],
+                [front_extension - offset, 0],
             ]
         )
 
-        # Rotate the boat according to the orientation
+        # Rotation matrix for anticlockwise rotation
         rotation_matrix = np.array(
             [
-                [np.cos(np.deg2rad(orientation)), -np.sin(np.deg2rad(orientation))],
+                [np.cos(np.deg2rad(orientation)), np.sin(np.deg2rad(orientation))],
                 [np.sin(np.deg2rad(orientation)), np.cos(np.deg2rad(orientation))],
             ]
         )
+
+        # Apply rotation
         rotated_boat = np.dot(boat_shape, rotation_matrix)
 
-        # Translate the boat to its position
-        translated_boat = rotated_boat + position
+        # Translate boat to its position
+        translated_boat = rotated_boat + np.array(position)
 
         # Draw the boat
         ax.plot(translated_boat[:, 0], translated_boat[:, 1], "k")
 
+        # Ensure the orientation line is drawn correctly
+        # Calculate a point along the orientation direction
+        direction = np.array([np.cos(np.deg2rad(orientation)), np.sin(np.deg2rad(orientation))])
+        line_start = np.array(position)
+        line_end = (
+            line_start + direction * boat_length
+        )  # Extend the line out from the boat's position
+
+        # Draw orientation line
+        ax.plot(
+            [line_start[0], line_end[0]], [line_start[1], line_end[1]], "black", linestyle="--"
+        )
+
     def visualize_forces(
         self, apparent_velocity, lift_force, drag_force, position=[0, 0], orientation=0
     ):
@@ -200,7 +250,7 @@ def visualize_forces(
             color="blue",
             scale=5,
             label="Apparent Velocity",
-            pivot="tip"
+            pivot="tip",
         )
         ax.quiver(
             position[0],
@@ -220,19 +270,28 @@ def visualize_forces(
             scale=5,
             label="Drag Force",
         )
-        orientation_rad = np.deg2rad(orientation) # Convert orientation to radians
+        orientation_rad = np.deg2rad(orientation)  # Convert orientation to radians
         ax.axline((0, 0), slope=np.tan(orientation_rad), color="black", linestyle="--")
 
         # Calculate angle for drag force
         drag_angle = np.arctan2(norm_drag_force[1], norm_drag_force[0])
-        
+
         # Determine start and end angles for the arc
         start_angle = np.rad2deg(orientation_rad)
         end_angle = np.rad2deg(drag_angle)
-        
+
         # Draw arc to represent angle between orientation and drag force
         radius = 0.05
-        arc = patches.Arc(position, 2*radius, 2*radius, angle=0, theta1=min(start_angle, end_angle), theta2=max(start_angle, end_angle), color='purple', label='Angle Arc')
+        arc = patches.Arc(
+            position,
+            2 * radius,
+            2 * radius,
+            angle=0,
+            theta1=min(start_angle, end_angle),
+            theta2=max(start_angle, end_angle),
+            color="purple",
+            label="Angle Arc",
+        )
         ax.add_patch(arc)
 
         ax.axis("equal")

diff --git a/package.xml b/package.xml
@@ -15,6 +15,7 @@
   <exec_depend>custom_interfaces</exec_depend>
   <exec_depend>ros2launch</exec_depend>
   <exec_depend>rosidl_parser</exec_depend>
+  <exec_depend>python3-matplotlib</exec_depend>
 
   <export>
     <build_type>ament_python</build_type>