add rotational inertial for kite (#100)

src/KiteModels.jl

@@ -61,6 +61,7 @@ export init_sim!, reset_sim!, next_step!, init_pos_vel, init_pos, model!
 export pos_kite, calc_height, calc_elevation, calc_azimuth, calc_heading, calc_course, calc_orient_quat  # getters
 export calc_azimuth_north, calc_azimuth_east
 export winch_force, lift_drag, cl_cd, lift_over_drag, unstretched_length, tether_length, v_wind_kite     # getters
+export calculate_rotational_inertia!
 export kite_ref_frame, orient_euler, spring_forces, upwind_dir, copy_model_settings, menu2
 import LinearAlgebra: norm
 
@@ -363,6 +364,56 @@ function calc_course(s::AKM, neg_azimuth=false)
     KiteUtils.calc_course(s.vel_kite, elevation, azimuth)
 end
 
+"""
+    calculate_rotational_inertia!(s::AKM, include_kcu::Bool=true, around_kcu::Bool=false)
+
+Calculate the rotational inertia (Ixx, Ixy, Ixz, Iyy, Iyz, Izz) for a kite model from settings. Modifies the kitemodel by initialising the masses.
+
+Parameters:
+- X: x-coordinates of the point masses.
+- Y: y-coordinates of the point masses.
+- Z: z-coordinates of the point masses.
+- M: masses of the point masses.
+- `include_kcu`: Include the kcu in the rotational intertia calculation?
+- `around_kcu`: Uses the kcu as the rotation point.
+
+Returns:  
+The tuple  Ixx, Ixy, Ixz, Iyy, Iyz, Izz where:
+- Ixx: rotational inertia around the x-axis.
+- Ixy: rotational inertia around the xy-plane.
+- Ixz: rotational inertia around the xz-plane.
+- Iyy: rotational inertia around the y-axis.
+- Iyz: rotational inertia around the yz-plane.
+- Izz: rotational inertia around the z-axis.
+
+"""
+function calculate_rotational_inertia!(s::AKM, include_kcu::Bool=true, around_kcu::Bool=false)
+    points = KiteUtils.get_particles(s.set.height_k, s.set.h_bridle, s.set.width, s.set.m_k, [0, 0, 0], [0, 0, -1], [10, 0, 0])
+
+    pos_matrix = [points[begin+1] points[begin+2] points[begin+3] points[begin+4] points[begin+5]]
+    X = pos_matrix[begin, :]
+    Y = pos_matrix[begin+1, :]
+    Z = pos_matrix[begin+2, :]
+
+    masses = init_masses!(s)
+    M = masses[s.set.segments+1:end]
+
+    if !include_kcu
+        X = X[begin+1:end]
+        Y = Y[begin+1:end]
+        Z = Z[begin+1:end]
+        M = M[begin+1:end]
+    end
+
+    if around_kcu
+        Ixx, Ixy, Ixz, Iyy, Iyz, Izz = KiteUtils.calculate_rotational_inertia(X, Y, Z, M, false, points[begin+1])
+    else
+        Ixx, Ixy, Ixz, Iyy, Iyz, Izz = KiteUtils.calculate_rotational_inertia(X, Y, Z, M)
+    end
+
+    Ixx, Ixy, Ixz, Iyy, Iyz, Izz
+end
+
 # mutable struct SysState{P}
 #     "time since start of simulation in seconds"
 #     time::Float64

test/runtests.jl

@@ -11,4 +11,5 @@ KiteUtils.set_data_path("")
     include("bench3.jl")
     include("bench4.jl")
     include("test_helpers.jl")
+    include("test_inertia_calculation.jl")
 end

test/test_inertia_calculation.jl

@@ -0,0 +1,53 @@
+using KiteModels
+using Test
+
+set = deepcopy(load_settings("system.yaml"))
+
+TEST_KCU = KCU(set)
+S = KPS4(TEST_KCU)
+
+@testset verbose=true "test_rotational_inertia" begin
+    @testset "kite including KCU around CoM " begin
+        Ixx, Ixy, Ixz, Iyy, Iyz, Izz = calculate_inertia_for_setting!(S, true, false)
+
+        @test isapprox(Ixx, 124.53,     rtol=0.005)
+        @test isapprox(Ixy, 0,          rtol=0.001)
+        @test isapprox(Ixz, -8.805,     rtol=0.007)
+        @test isapprox(Iyy, 111.227,    rtol=0.006)
+        @test isapprox(Iyz, 0,          rtol=0.001)
+        @test isapprox(Izz, 19.516,     rtol=0.001)
+    end
+
+    @testset "kite no KCU around CoM" begin
+        Ixx, Ixy, Ixz, Iyy, Iyz, Izz = calculate_inertia_for_setting!(S, false, false)
+
+        @test isapprox(Ixx, 21.5607,    rtol=0.001)
+        @test isapprox(Ixy, 0,          rtol=0.001)
+        @test isapprox(Ixz, 1.589,      rtol=0.001)
+        @test isapprox(Iyy, 7.2073,     rtol=0.001)
+        @test isapprox(Iyz, 0,          rtol=0.001)
+        @test isapprox(Izz, 18.4558,    rtol=0.001)
+    end
+
+    @testset "kite including KCU around KCU" begin
+        Ixx, Ixy, Ixz, Iyy, Iyz, Izz = calculate_inertia_for_setting!(S, true, true)
+
+        @test isapprox(Ixx, 200.533,    rtol=0.001) 
+        @test isapprox(Ixy, 0,          rtol=0.001) 
+        @test isapprox(Ixz, -16.478,    rtol=0.001)
+        @test isapprox(Iyy, 188.004,    rtol=0.001)
+        @test isapprox(Iyz, 0,          rtol=0.001)
+        @test isapprox(Izz, 20.2907,    rtol=0.001)
+    end
+
+    @testset "kite no KCU around KCU" begin
+        Ixx, Ixy, Ixz, Iyy, Iyz, Izz = calculate_inertia_for_setting!(S, false, true)
+
+        @test isapprox(Ixx, 200.533,    rtol=0.001) 
+        @test isapprox(Ixy, 0,          rtol=0.001) 
+        @test isapprox(Ixz, -16.478,    rtol=0.001)
+        @test isapprox(Iyy, 188.004,    rtol=0.001)
+        @test isapprox(Iyz, 0,          rtol=0.001)
+        @test isapprox(Izz, 20.2907,    rtol=0.001)
+    end
+end