Add option to align frames

Author: 1-Bart-1

examples/ram_air_kite.jl

@@ -2,7 +2,7 @@ using ControlPlots
 using VortexStepMethod
 using LinearAlgebra
 
-PLOT = true
+PLOT = false
 USE_TEX = false
 DEFORM = false
 
@@ -46,7 +46,7 @@ set_va!(body_aero, vel_app)
 PLOT && plot_polar_data(body_aero)
 
 # Plotting geometry
-PLOT && plot_geometry(
+true && plot_geometry(
     body_aero,
     "";
     data_type=".svg",

src/kite_geometry.jl

@@ -155,28 +155,63 @@ function create_interpolations(vertices, circle_center_z, radius, gamma_tip)
     return (le_interp, te_interp, area_interp)
 end
 
-# Makes the zero coordinates lie on the com
+"""
+    center_to_com!(vertices, faces)
+
+Calculate center of mass of a mesh and translate vertices so that COM is at origin.
+
+# Arguments
+- `vertices`: Vector of 3D point coordinates
+- `faces`: Vector of vertex indices for each face (can be triangular or non-triangular)
+
+# Returns
+- Vector representing the original center of mass before translation
+
+# Notes
+- Non-triangular faces are automatically triangulated into triangles
+- Assumes uniform surface density
+"""
 function center_to_com!(vertices, faces)
     area_total = 0.0
     com = zeros(3)
 
     for face in faces
-        v1 = vertices[face[1]]
-        v2 = vertices[face[2]]
-        v3 = vertices[face[3]]
-        
-        # Calculate triangle area and centroid
-        normal = cross(v2 - v1, v3 - v1)
-        area = norm(normal) / 2
-        centroid = (v1 + v2 + v3) / 3
-        
-        area_total += area
-        com += area * centroid
+        if length(face) == 3
+            # Triangle case
+            v1 = vertices[face[1]]
+            v2 = vertices[face[2]]
+            v3 = vertices[face[3]]
+            
+            # Calculate triangle area and centroid
+            normal = cross(v2 - v1, v3 - v1)
+            area = norm(normal) / 2
+            centroid = (v1 + v2 + v3) / 3
+            
+            area_total += area
+            com += area * centroid
+        else
+            println("found non-triangular face")
+            # Non-triangular face case - triangulate the face
+            v1 = vertices[face[1]]  # Use first vertex as pivot
+            for i in 2:length(face)-1
+                v2 = vertices[face[i]]
+                v3 = vertices[face[i+1]]
+                
+                # Calculate triangle area and centroid for this triangle
+                normal = cross(v2 - v1, v3 - v1)
+                area = norm(normal) / 2
+                centroid = (v1 + v2 + v3) / 3
+                
+                area_total += area
+                com += area * centroid
+            end
+        end
     end
 
     com = com / area_total
     !(abs(com[2]) < 0.01) && throw(ArgumentError("Center of mass $com of .obj file has to lie on the xz-plane."))
     @info "Centering vertices of .obj file to the center of mass: $com"
+    com[2] = 0.0
     for v in vertices
         v .-= com
     end
@@ -245,6 +280,49 @@ function calculate_inertia_tensor(vertices, faces, mass, com)
     return (mass / total_area) * I / 3
 end
 
+function calc_inertia_y_rotation(I_b_tensor)
+    # Function for nonlinear solver - off-diagonal element should be zero
+    function eq!(F, theta, _)
+        # Rotation matrix around y-axis
+        R_y = [
+            cos(theta[1])  0  sin(theta[1]);
+            0              1  0;
+            -sin(theta[1]) 0  cos(theta[1])
+        ]
+        # Transform inertia tensor
+        I_rotated = R_y * I_b_tensor * R_y'
+        # We want the off-diagonal xz elements to be zero
+        F[1] = I_rotated[1,3] 
+    end
+    
+    theta0 = [0.0]
+    prob = NonlinearProblem(eq!, theta0, nothing)
+    sol = NonlinearSolve.solve(prob, NewtonRaphson())
+    theta_opt = sol.u[1]
+    
+    R_b_p = [
+        cos(theta_opt)  0  sin(theta_opt);
+        0               1  0;
+        -sin(theta_opt) 0  cos(theta_opt)
+    ]
+    @show theta_opt
+    # Calculate diagonalized inertia tensor
+    I_diag = R_b_p * I_b_tensor * R_b_p'
+    @show I_diag
+    @assert isapprox(I_diag[1,3], 0.0, atol=1e-5)
+    @show R_b_p
+    return I_diag, R_b_p
+end
+
+function align_to_principal!(vertices, I_b_tensor)
+    I_diag, R_b_p = calc_inertia_y_rotation(I_b_tensor)
+    for v in vertices
+        v .= R_b_p * v # transform body frame vertices to principal frame
+    end
+    # the rotation between body frame and principal frame is now zero
+    return I_diag
+end
+
 """
     interpolate_matrix_nans!(matrix::Matrix{Float64})
 
@@ -367,7 +445,7 @@ Constructor for a [RamAirWing](@ref) that allows to use an `.obj` and a `.dat` f
 """
 function RamAirWing(obj_path, dat_path; alpha=0.0, crease_frac=0.75, wind_vel=10., mass=1.0, 
                   n_panels=54, n_sections=n_panels+1, spanwise_panel_distribution=UNCHANGED, 
-                  spanwise_direction=[0.0, 1.0, 0.0], remove_nan=true,
+                  spanwise_direction=[0.0, 1.0, 0.0], remove_nan=true, align_to_principal=false,
                   alpha_range=deg2rad.(-5:1:20), delta_range=deg2rad.(-5:1:20) 
                   )
 
@@ -382,38 +460,42 @@ function RamAirWing(obj_path, dat_path; alpha=0.0, crease_frac=0.75, wind_vel=10
     (!isfile(obj_path)) && error("OBJ file not found: $obj_path")
     info_path = obj_path[1:end-4] * "_info.bin"
 
-    if !ispath(polar_path) || !ispath(info_path)
+    if true || !ispath(info_path)
         @info "Reading $obj_path"
         vertices, faces = read_faces(obj_path)
         center_of_mass = center_to_com!(vertices, faces)
         inertia_tensor = calculate_inertia_tensor(vertices, faces, mass, zeros(3))
+        align_to_principal && align_to_principal!(vertices, inertia_tensor)
 
         circle_center_z, radius, gamma_tip = find_circle_center_and_radius(vertices)
         le_interp, te_interp, area_interp = create_interpolations(vertices, circle_center_z, radius, gamma_tip)
         @info "Writing $info_path"
         serialize(info_path, (inertia_tensor, center_of_mass, circle_center_z, radius, gamma_tip, 
             le_interp, te_interp, area_interp))
-
-        width = 2gamma_tip * radius
-        area = area_interp(gamma_tip)
-        @eval Main begin
-            foil_path, polar_path, v_wind, area, width, x_turn, alpha_range, delta_range =
-                $dat_path, $polar_path, $wind_vel, $gamma_tip, $width, $crease_frac, $alpha_range, $delta_range
-            include(joinpath(dirname(@__FILE__), "../scripts/polars.jl"))
-        end
     end
 
-    @info "Loading polars and kite info from $polar_path and $info_path"
+    @info "Loading kite info from $info_path and polars from $polar_path"
     try
+        (inertia_tensor::Matrix, center_of_mass::Vector, circle_center_z::Real,
+            radius::Real, gamma_tip::Real, le_interp, te_interp, area_interp) = deserialize(info_path)
+
+        if !ispath(polar_path)
+            width = 2gamma_tip * radius
+            area = area_interp(gamma_tip)
+            @eval Main begin
+                foil_path, polar_path, v_wind, area, width, x_turn, alpha_range, delta_range =
+                    $dat_path, $polar_path, $wind_vel, $gamma_tip, $width, $crease_frac, $alpha_range, $delta_range
+                include(joinpath(dirname(@__FILE__), "../scripts/polars.jl"))
+            end
+        end
+
         (alpha_range, delta_range, cl_matrix::Matrix, cd_matrix::Matrix, cm_matrix::Matrix) = deserialize(polar_path)
         if remove_nan
             interpolate_matrix_nans!(cl_matrix)
             interpolate_matrix_nans!(cd_matrix)
             interpolate_matrix_nans!(cm_matrix)
         end
-        (inertia_tensor::Matrix, center_of_mass::Vector, circle_center_z::Real,
-            radius::Real, gamma_tip::Real, le_interp, te_interp, area_interp) = deserialize(info_path)
-    
+
         # Create sections
         sections = Section[]
         refined_sections = Section[]
@@ -426,14 +508,15 @@ function RamAirWing(obj_path, dat_path; alpha=0.0, crease_frac=0.75, wind_vel=10
             push!(refined_sections, Section(LE_point, TE_point, POLAR_MATRICES, aero_data))
             push!(non_deformed_sections, Section(LE_point, TE_point, POLAR_MATRICES, aero_data))
         end
-    
+
         RamAirWing(n_panels, spanwise_panel_distribution, spanwise_direction, sections, 
             refined_sections, remove_nan, non_deformed_sections,
             mass, circle_center_z, gamma_tip, inertia_tensor, center_of_mass, radius,
             le_interp, te_interp, area_interp, zeros(n_panels), zeros(n_panels))
-    catch e
+
+    catch
         if e isa BoundsError
-            @error "Delete $polar_path and $info_path and try again."
+            @error "Delete $info_path and $polar_path and try again."
         end
         rethrow(e)
     end
@@ -486,6 +569,6 @@ Rotate vector v around axis k by angle θ using Rodrigues' rotation formula.
 """
 function rotate_v_around_k(v, k, θ)
     k = normalize(k)
-    v_rot = v * cos(θ) + (k × v) * sin(θ)  + k * (k ⋅ v) * (1 - cos(θ))
+    v_rot = v * cos(θ) + (k × v) * sin(θ) + k * (k ⋅ v) * (1 - cos(θ))
     return v_rot
 end