minor updates

FraCpl · Oct 10, 2024 · a120fbb · a120fbb
1 parent cd46889
commit a120fbb
Show file tree

Hide file tree

Showing 3 changed files with 42 additions and 26 deletions.
diff --git a/src/SpacecraftBuilder.jl b/src/SpacecraftBuilder.jl
@@ -7,7 +7,7 @@ using LinearAlgebra
 using GLMakie
 using GeometryBasics
 
-export verifyInertia, randomInertia, translateInertia, rotateInertia
+export verifyInertia, randomInertia, translateInertia, rotateInertia, massMatrix2mci
 include("utils.jl")
 
 export NoGeometry, PlanarPlate, Cuboid, Cylinder, Sphere

diff --git a/src/scBuild.jl b/src/scBuild.jl
@@ -41,37 +41,39 @@ function build(elements::Vector; plotModel=false)
 
     # Cycle through different elements
     for el in elements
-        ID = ID*" + "*el.ID
-        mass += el.mass
-        posOG_O .+= el.mass*el.posOG_O
-        inertiaO_O .+= el.inertiaO_O
+        if el.mass > 0.0
+            ID = ID*" + "*el.ID
+            mass += el.mass
+            posOG_O .+= el.mass*el.posOG_O
+            inertiaO_O .+= el.inertiaO_O
 
-        if typeof(el) == FlexibleElement
-            LO_O = [LO_O; copy(el.LO_O)]
-            freq = [freq; copy(el.freq)]
-            damp = [damp; copy(el.damp)]
+            if typeof(el) == FlexibleElement
+                LO_O = [LO_O; copy(el.LO_O)]
+                freq = [freq; copy(el.freq)]
+                damp = [damp; copy(el.damp)]
 
-            if !hasFlex
-                # Remove NaN values
-                LO_O = LO_O[2:end, :]
-                freq = freq[2:end]
-                damp = damp[2:end]
-                hasFlex = true
+                if !hasFlex
+                    # Remove NaN values
+                    LO_O = LO_O[2:end, :]
+                    freq = freq[2:end]
+                    damp = damp[2:end]
+                    hasFlex = true
+                end
             end
-        end
 
-        if plotModel
-            if typeof(el.geometry) == Cuboid
-                if isFirst
-                    model = cuboidModel(el.geometry.lx, el.geometry.ly, el.geometry.lz; pos_I=el.posOG_O, R_IB=el.R_OE)
-                    isFirst = false
-                else
-                    model = mergeMesh(model, cuboidModel(el.geometry.lx, el.geometry.ly, el.geometry.lz; pos_I=el.posOG_O, R_IB=el.R_OE))
+            if plotModel
+                if typeof(el.geometry) == Cuboid
+                    if isFirst
+                        model = cuboidModel(el.geometry.lx, el.geometry.ly, el.geometry.lz; pos_I=el.posOG_O, R_IB=el.R_OE)
+                        isFirst = false
+                    else
+                        model = mergeMesh(model, cuboidModel(el.geometry.lx, el.geometry.ly, el.geometry.lz; pos_I=el.posOG_O, R_IB=el.R_OE))
+                    end
+                    #mesh!(p3d, cuboidModel(el.geometry.lx, el.geometry.ly, el.geometry.lz; pos_I=el.posOG_O, R_IB=el.R_OE); color=:lawngreen, alpha=0.5)
                 end
-                #mesh!(p3d, cuboidModel(el.geometry.lx, el.geometry.ly, el.geometry.lz; pos_I=el.posOG_O, R_IB=el.R_OE); color=:lawngreen, alpha=0.5)
+                plotframe!(p3d, el.posOG_O, el.R_OE, 2.0)
+                scatter!(p3d, el.posOE_O[1], el.posOE_O[2], el.posOE_O[3]; markersize=10)   # This gets hidden by the mesh!
             end
-            plotframe!(p3d, el.posOG_O, el.R_OE, 2.0)
-            scatter!(p3d, el.posOE_O[1], el.posOE_O[2], el.posOE_O[3]; markersize=10)   # This gets hidden by the mesh!
         end
     end
 

diff --git a/src/utils.jl b/src/utils.jl
@@ -71,3 +71,17 @@ function randomInertia(Jnom::Matrix{Float64}, err::Vector{Float64})::Matrix{Floa
     A = (1.0 .+ err).*(sum(Jdiag)/2 .- Jdiag)
     return R*diagm(sum(A) .- A)*R'
 end
+
+function massMatrix2mci(MQ_B)
+    if !isdiag(MQ_B[1:3, 1:3])
+        @warn("Waring: mass not diagonal")
+    end
+    if norm(MQ_B - MQ_B') > 1e-9
+        @warn("Mass matrix not symmetrical")
+    end
+    mass = MQ_B[1, 1]
+    JQ_B = MQ_B[4:6, 4:6]
+    verifyInertia(JQ_B)
+    rQG_B = [-MQ_B[5, 3]; MQ_B[4, 3]; -MQ_B[4, 2]]./mass
+    return mass, rQG_B, JQ_B
+end