add rotational inertia computation to kiteutils and add tests

src/KiteUtils.jl

@@ -56,6 +56,7 @@ export azn2azw, calc_heading_w, calc_heading, calc_course                # geome
 export calc_orient_rot, is_right_handed_orthonormal, enu2ned, ned2enu
 export set_data_path, get_data_path, load_settings, copy_settings        # functions for reading and copying parameters
 export se, se_dict, update_settings, wc_settings, fpc_settings, fpp_settings
+export calculate_rotational_inertia
 
 """
     const MyFloat = Float32
@@ -458,6 +459,62 @@ end
 
 include("_load_log.jl")
 
+
+"""
+    calculate_rotational_inertia(X::Vector, Y::Vector, Z::Vector, M::Vector, around_center_of_mass::Bool=true, 
+    rotation_point::Vector=[0, 0, 0])
+
+Calculate the rotational inertia (Ixx, Ixy, Ixz, Iyy, Iyz, Izz) of a colection of point masses around a point. 
+By default this point is the center of mass which will be calculated, but any point can be given to rotation_point.
+
+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.
+- around_center_of_mass: Calculate the rotational inertia around the center of mass?
+- rotation_point: Rotation point used if not rotating around the center of mass.
+"""
+function calculate_rotational_inertia(X::Vector, Y::Vector, Z::Vector, M::Vector, around_center_of_mass::Bool=true, 
+    rotation_point::Vector=[0, 0, 0])
+    @assert size(X) == size(Y) == size(Z) == size(M)
+
+    if around_center_of_mass
+        # First loop to determine the center of mass
+        x_com = y_com = z_com = m_total = 0.0
+        for (x, y, z, m) in zip(X, Y, Z, M)
+            x_com += x * m
+            y_com += y * m
+            z_com += z * m
+            m_total += m 
+        end
+
+        x_com = x_com / m_total
+        y_com = y_com / m_total
+        z_com = z_com / m_total
+    else
+        x_com = rotation_point[begin]
+        y_com = rotation_point[begin+1]
+        z_com = rotation_point[begin+2]
+    end
+
+    Ixx = Ixy = Ixz = Iyy = Iyz = Izz = 0
+
+    # Second loop using the distance between the point and the center of mass
+    for (x, y, z, m) in zip(X .- x_com, Y .- y_com, Z .- z_com, M)
+        Ixx += m * (y^2 + z^2)
+        Iyy += m * (x^2 + z^2)
+        Izz += m * (x^2 + y^2)
+
+        Ixy += m * x * y
+        Ixz += m * x * z
+        Iyz += m * y * z
+    end
+
+    Ixx, Ixy, Ixz, Iyy, Iyz, Izz
+end
+
+
 function test(save=false)
     if save
         log_to_save=demo_log(7)

test/runtests.jl

@@ -61,6 +61,7 @@ include("test_logger.jl")
 include("test_transformations.jl")
 include("test_orientation.jl")
 include("test_azimuth.jl")
+include("test_rotational_inertia.jl")
 include("bench.jl")
 end
 nothing

test/test_rotational_inertia.jl

@@ -0,0 +1,131 @@
+using Test
+using KiteUtils
+
+
+@testset "point mass" begin
+    X = [20]
+    Y = [511]
+    Z = [123]
+    M = [21]
+
+    Ixx, Ixy, Ixz, Iyy, Iyz, Izz = KiteUtils.calculate_rotational_inertia(X, Y, Z, M)
+
+    @test Ixx == 0
+    @test Ixy == 0
+    @test Ixz == 0
+    @test Iyy == 0
+    @test Iyz == 0
+    @test Izz == 0
+end
+
+@testset "line in x" begin
+    X = [-10, 10]
+    Y = [20, 20]
+    Z = [-3, -3]
+    M = [7, 3]
+
+    Ixx, Ixy, Ixz, Iyy, Iyz, Izz = KiteUtils.calculate_rotational_inertia(X, Y, Z, M)
+
+    @test Ixx == 0
+    @test Ixy == 0
+    @test Ixz == 0
+    @test Iyy == 840
+    @test Iyz == 0
+    @test Izz == 840
+end
+
+@testset "line in y" begin
+    X = [10, 10]
+    Y = [-20, 20]
+    Z = [-3, -3]
+    M = [7, 3]
+
+    Ixx, Ixy, Ixz, Iyy, Iyz, Izz = KiteUtils.calculate_rotational_inertia(X, Y, Z, M)
+
+    @test Ixx == 3360
+    @test Ixy == 0
+    @test Ixz == 0
+    @test Iyy == 0
+    @test Iyz == 0
+    @test Izz == 3360
+end
+
+@testset "line in z" begin
+    X = [10, 10]
+    Y = [20, 20]
+    Z = [3, -3]
+    M = [7, 3]
+
+    Ixx, Ixy, Ixz, Iyy, Iyz, Izz = KiteUtils.calculate_rotational_inertia(X, Y, Z, M)
+
+    @test Ixx == 75.6
+    @test Ixy == 0
+    @test Ixz == 0
+    @test Iyy == 75.6
+    @test Iyz == 0
+    @test Izz == 0
+end
+
+@testset "line in xy" begin
+    X = [-10, 10]
+    Y = [-20, 20]
+    Z = [-3, -3]
+    M = [7, 3]
+
+    Ixx, Ixy, Ixz, Iyy, Iyz, Izz = KiteUtils.calculate_rotational_inertia(X, Y, Z, M)
+
+    @test Ixx == 3360
+    @test Ixy == 1680
+    @test Ixz == 0
+    @test Iyy == 840
+    @test Iyz == 0
+    @test Izz == 4200
+end
+
+@testset "line in xz" begin
+    X = [-10, 10]
+    Y = [-3, -3]
+    Z = [-20, 20]
+    M = [7, 3]
+
+    Ixx, Ixy, Ixz, Iyy, Iyz, Izz = KiteUtils.calculate_rotational_inertia(X, Y, Z, M)
+
+    @test Ixx == 3360
+    @test Ixy == 0
+    @test Ixz == 1680
+    @test Iyy == 4200
+    @test Iyz == 0
+    @test Izz == 840
+end
+
+@testset "line in yz" begin
+    X = [-3, -3]
+    Y = [-10, 10]
+    Z = [-20, 20]
+    M = [7, 3]
+
+    Ixx, Ixy, Ixz, Iyy, Iyz, Izz = KiteUtils.calculate_rotational_inertia(X, Y, Z, M)
+
+    @test Ixx == 4200 
+    @test Ixy == 0
+    @test Ixz == 0
+    @test Iyy == 3360
+    @test Iyz == 1680
+    @test Izz == 840
+end
+
+@testset "line in xyz" begin
+    X = [-10, 10]
+    Y = [-20, 20]
+    Z = [3, -3]
+    M = [7, 3]
+
+    Ixx, Ixy, Ixz, Iyy, Iyz, Izz = KiteUtils.calculate_rotational_inertia(X, Y, Z, M)
+
+    @test isapprox(Ixx, 3435.6)
+    @test Ixy == 1680
+    @test Ixz == -252
+    @test isapprox(Iyy, 915.6)
+    @test Iyz == -504
+    @test Izz == 4200
+end