Add capsule test for local AABB computation (#423)

flexible-collision-library · Nov 26, 2019 · 6aeb7a1 · 6aeb7a1
1 parent cb60d9e
commit 6aeb7a1
Showing 1 changed file with 30 additions and 7 deletions.
diff --git a/test/geometry/shape/test_capsule.cpp b/test/geometry/shape/test_capsule.cpp
@@ -59,6 +59,16 @@ std::vector<paird> get_test_sizes() {
   };
 }
 
+template <typename S>
+void testLocalAABBComputation(Capsule<S>& shape, S tol) {
+  shape.computeLocalAABB();
+
+  S r = shape.radius;
+  S l = shape.lz;
+  EXPECT_NEAR(shape.aabb_radius, Vector3<S>(r, r, 0.5 * l + r).norm(), tol);
+  EXPECT_TRUE(CompareMatrices(shape.aabb_center, Vector3<S>(0, 0, 0), tol));
+}
+
 template <typename S>
 void testVolumeComputation(const Capsule<S>& shape, S tol) {
   S r = shape.radius;
@@ -108,15 +118,29 @@ void testMomentOfInertiaComputation(const Capsule<S>& shape, S tol) {
   EXPECT_TRUE(shape.computeMomentofInertia().isApprox(I_cap, tol));
 }
 
+GTEST_TEST(Capsule, LocalAABBComputation_Capsule) {
+  for (paird pair : get_test_sizes()) {
+    double rd = pair.first;
+    double ld = pair.second;
+    Capsuled capsule_d(rd, ld);
+    testLocalAABBComputation(capsule_d, 1e-15);
+
+    float rf = static_cast<float>(pair.first);
+    float lf = static_cast<float>(pair.second);
+    Capsulef capsule_f(rf, lf);
+    testLocalAABBComputation(capsule_f, 1e-8f);
+  }
+}
+
 GTEST_TEST(Capsule, Volume_Capsule) {
   for (paird pair : get_test_sizes()) {
     double rd = pair.first;
     double ld = pair.second;
     Capsuled capsule_d(rd, ld);
     testVolumeComputation(capsule_d, 1e-15);
 
-    double rf = static_cast<float>(pair.first);
-    double lf = static_cast<float>(pair.second);
+    float rf = static_cast<float>(pair.first);
+    float lf = static_cast<float>(pair.second);
     Capsulef capsule_f(rf, lf);
     testVolumeComputation(capsule_f, 1e-8f);
   }
@@ -131,8 +155,8 @@ GTEST_TEST(Capsule, CenterOfMass_Capsule) {
     Capsuled capsule_d(rd, ld);
     EXPECT_TRUE(CompareMatrices(capsule_d.computeCOM(), comd, 0.));
 
-    double rf = static_cast<float>(pair.first);
-    double lf = static_cast<float>(pair.second);
+    float rf = static_cast<float>(pair.first);
+    float lf = static_cast<float>(pair.second);
     Capsulef capsule_f(rf, lf);
     EXPECT_TRUE(CompareMatrices(capsule_f.computeCOM(), comf, 0.f));
   }
@@ -145,8 +169,8 @@ GTEST_TEST(Capsule, MomentOfInertia_Capsule) {
     Capsuled capsule_d(rd, ld);
     testMomentOfInertiaComputation(capsule_d, 1e-14);
 
-    double rf = static_cast<float>(pair.first);
-    double lf = static_cast<float>(pair.second);
+    float rf = static_cast<float>(pair.first);
+    float lf = static_cast<float>(pair.second);
     Capsulef capsule_f(rf, lf);
     testMomentOfInertiaComputation(capsule_f, 1e-6f);
   }
@@ -160,4 +184,3 @@ int main(int argc, char *argv[]) {
   ::testing::InitGoogleTest(&argc, argv);
   return RUN_ALL_TESTS();
 }
-