Merge pull request #64 from jenkinshrg/feature

add google test

CMakeLists.txt

@@ -20,6 +20,12 @@ endif()
 
 project(OpenHRP)
 
+option(BUILD_GOOGLE_TEST "Build google test." OFF)
+if(BUILD_GOOGLE_TEST)
+  enable_testing()
+  add_subdirectory(/usr/src/gtest ${CMAKE_CURRENT_BINARY_DIR}/gtest)
+endif()
+
 set(OPENHRP_VERSION_MAJOR 3)
 set(OPENHRP_VERSION_MINOR 1)
 set(OPENHRP_VERSION_MICRO 8)
@@ -598,4 +604,3 @@ else()
 endif()
 ")
 endif()
-

hrplib/hrpUtil/CMakeLists.txt

@@ -102,3 +102,10 @@ endif()
 hrplib_install_macro(${target} ${HRPUTIL_VERSION})
 
 install(FILES ${headers} DESTINATION ${RELATIVE_HEADERS_INSTALL_PATH}/hrpUtil)
+
+if(BUILD_GOOGLE_TEST)
+  add_executable(testEigen3d testEigen3d.cpp Eigen3d.cpp)
+  target_link_libraries(testEigen3d gtest gtest_main pthread)
+  set_target_properties(testEigen3d PROPERTIES COMPILE_FLAGS "-g -O0 -coverage" LINK_FLAGS "-g -O0 -coverage")
+  add_test(testEigen3d ${EXECUTABLE_OUTPUT_PATH}/testEigen3d)
+endif()

hrplib/hrpUtil/testEigen3d.cpp

@@ -0,0 +1,150 @@
+#include <gtest/gtest.h>
+#include "Eigen3d.h"
+
+using namespace hrp;
+
+TEST(TestEigen3d, test1)
+{
+    Matrix33 m;
+    Vector3 v;
+    m << 1.0,   0,   0,
+           0, 1.0,   0,
+           0,   0, 1.0;
+    v = omegaFromRot(m);
+    EXPECT_EQ(v[0], 0);
+    EXPECT_EQ(v[1], 0);
+    EXPECT_EQ(v[2], 0);
+}
+
+/*
+TEST(TestEigen3d, test2)
+{
+    Matrix33 m;
+    Vector3 v;
+    m << 1.0,   0,   0,
+           0, 1.0,   0,
+           0,   0, 1.0-1.0e-12;
+    v = omegaFromRot(m);
+    EXPECT_EQ(v[0], 0);
+    EXPECT_EQ(v[1], 0);
+    EXPECT_EQ(v[2], 0);
+}
+
+TEST(TestEigen3d, test3)
+{
+    Matrix33 m;
+    Vector3 v;
+    m << 1.0,   0,   0,
+           0, 1.0,   0,
+           0,   0, 1.0+1.0e-12;
+    v = omegaFromRot(m);
+    EXPECT_EQ(v[0], 0);
+    EXPECT_EQ(v[1], 0);
+    EXPECT_EQ(v[2], 0);
+}
+
+TEST(TestEigen3d, test4)
+{
+    Matrix33 m;
+    Vector3 v;
+    m << 1.0,   0,   0,
+           0, 1.0,   0,
+           0,   0, 1.0-1.0e-6;
+    v = omegaFromRot(m);
+    EXPECT_EQ(v[0], 0);
+    EXPECT_EQ(v[1], 0);
+    EXPECT_EQ(v[2], 0);
+}
+
+TEST(TestEigen3d, test5)
+{
+    Matrix33 m;
+    Vector3 v;
+    m << 1.0,   0,   0,
+           0, 1.0,   0,
+           0,   0, 1.0+1.0e-6;
+    v = omegaFromRot(m);
+    EXPECT_EQ(v[0], 0);
+    EXPECT_EQ(v[1], 0);
+    EXPECT_EQ(v[2], 0);
+}
+*/
+
+TEST(TestEigen3d, test6)
+{
+    Matrix33 m;
+    Vector3 v;
+    m <<-1.0,   0,   0,
+           0, 1.0,   0,
+           0,   0,-1.0;
+    v = omegaFromRot(m);
+    EXPECT_EQ(v[0], 0);
+    EXPECT_EQ(v[1], M_PI);
+    EXPECT_EQ(v[2], 0);
+}
+
+/*
+TEST(TestEigen3d, test7)
+{
+    Matrix33 m;
+    Vector3 v;
+    m <<-1.0,   0,   0,
+           0, 1.0,   0,
+           0,   0,-1.0-::std::numeric_limits<double>::epsilon();
+    v = omegaFromRot(m);
+    EXPECT_EQ(v[0], 0);
+    EXPECT_EQ(v[1], 0);
+    EXPECT_EQ(v[2], 0);
+}
+
+TEST(TestEigen3d, test8)
+{
+    Matrix33 m;
+    Vector3 v;
+    m <<-1.0,   0,   0,
+           0, 1.0,   0,
+           0,   0,-1.0+::std::numeric_limits<double>::epsilon();
+    v = omegaFromRot(m);
+    EXPECT_EQ(v[0], 0);
+    EXPECT_EQ(v[1], 0);
+    EXPECT_EQ(v[2], 0);
+}
+
+TEST(TestEigen3d, test9)
+{
+    Matrix33 m;
+    Vector3 v;
+    for (int i = 1; i <= 16; i++) {
+      for (int j = 1; j <= 16; j++) {
+        for (int k = 1; k <= 16; k++) {
+          Vector3 rpy(pow(0.1, i), pow(0.1, j), pow(0.1, k));
+          m = rotFromRpy(rpy);
+          v = omegaFromRot(m);
+          EXPECT_FALSE(isnan(v[0]));
+          EXPECT_FALSE(isnan(v[1]));
+          EXPECT_FALSE(isnan(v[2]));
+        }
+      }
+    }
+}
+*/
+
+TEST(TestEigen3d, test10)
+{
+    Matrix33 m;
+    Vector3 v;
+    for (int i = 0; i <= 90; i++) {
+      for (int j = 0; j <= 90; j++) {
+        for (int k = 0; k <= 90; k++) {
+          double r = i*M_PI/180;
+          double p = j*M_PI/180;
+          double y = k*M_PI/180;
+          m = rotFromRpy(r, p, y);
+          v = omegaFromRot(m);
+          EXPECT_FALSE(isnan(v[0]));
+          EXPECT_FALSE(isnan(v[1]));
+          EXPECT_FALSE(isnan(v[2]));
+        }
+      }
+    }
+}