Test: Rigid bodies

iagoleal · iagoleal · commit 077035f8955c · 2020-05-22T00:37:58.000-03:00
diff --git a/test/Project.toml b/test/Project.toml
@@ -1,3 +1,4 @@
 [deps]
+LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e"
 Random = "9a3f8284-a2c9-5f02-9a11-845980a1fd5c"
 Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40"
diff --git a/test/rigid-bodies.jl b/test/rigid-bodies.jl
@@ -0,0 +1,35 @@
+using DeformableBodies
+using Test
+using Random
+import LinearAlgebra: norm
+
+# Uniform Distribution on Sphere
+function unif_sphere(m)
+    v = randn(m)
+    return v / norm(v)
+end
+
+approx_zero_array(A) = all(isapprox.(A, 0.0, atol=1e-6))
+
+@testset "Rigid Bodies" for i in 1:5
+    @testset "Zero angular momentum" begin
+        npts = rand(10:100)
+        # A rigid body contained on a ball
+        r_0 = [PointMass(rand()*10, unif_sphere(3)) for i in 1:npts]
+        bigV_r0 = vcat(pos.(r_0)...)
+        model = Model( t -> r_0 # Constant trajectories
+                     , (0., 1.)
+                     , quaternion(1)
+                     , [0.,0.,0.]
+                     )
+        rotbodies, R, Π = solve!(model)
+
+        for t in range(0., 1., length=30)
+            bigV_rt = vcat(pos.(model.inertialframe(t))...)
+            @test R(t) ≈ 1 atol=1e-7
+            @test approx_zero_array(Π(t))
+            @test isapprox(bigV_r0, bigV_rt, atol=1e-6)
+        end
+    end
+end
+
diff --git a/test/runtests.jl b/test/runtests.jl
@@ -11,4 +11,6 @@ Random.seed!(12342352154)
 @testset "DeformableBodies Package" begin
     @info "Testing Quaternions"
     include(filepath("quaternions.jl"))
+    @info "Testing dynamics for Rigid Bodies"
+    include(filepath("rigid-bodies.jl"))
 end
