Function TEST

uav_simulator/Utils/uav_utils/src/uav_utils_test.cpp:10–18

#define FLOAT_EPS 1.0e-6

TEST(GeometryUtilsDouble, Rotation) {
  Vector3d v(0.1, 0.2, 0.3);
  ASSERT_TRUE(v.isApprox(R_to_ypr(ypr_to_R(v)), DOUBLE_EPS));
  ASSERT_TRUE(v.isApprox(quaternion_to_ypr(ypr_to_quaternion(v)), DOUBLE_EPS));
  ASSERT_TRUE(v.isApprox(R_to_ypr(rotz(v(0)) * roty(v(1)) * rotx(v(2))), DOUBLE_EPS));

  double yaw = 30.0;
  ASSERT_DOUBLE_EQ(30.0, toDeg(get_yaw_from_quaternion(yaw_to_quaternion(toRad(yaw)))));
}

TEST(GeometryUtilsFloat, Rotation) {
  Vector3f v(0.1, 0.2, 0.3);