b555fb5d6a
All checks were successful
Build and Run C++ Unit Tests / build-and-test (push) Successful in 2m2s
139 lines
4.4 KiB
C++
139 lines
4.4 KiB
C++
#if GTEST
|
|
#include <gtest/gtest.h>
|
|
#include <math.h>
|
|
#include <limits>
|
|
|
|
#include "SwingTwist.h"
|
|
|
|
#define FLOAT_INFINITY std::numeric_limits<float>::infinity()
|
|
|
|
using BaseTypes = ::testing::Types<float, signed short>;
|
|
|
|
template <typename T>
|
|
class SwingTwistTests : public ::testing::Test {};
|
|
TYPED_TEST_SUITE(SwingTwistTests, BaseTypes);
|
|
|
|
TYPED_TEST(SwingTwistTests, Quaternion) {
|
|
using T = TypeParam;
|
|
using SwingTwist = SwingTwistOf<T>;
|
|
|
|
Quaternion q;
|
|
SwingTwist s;
|
|
Quaternion rq;
|
|
|
|
q = Quaternion::identity;
|
|
s = SwingTwist::FromQuaternion(q);
|
|
rq = s.ToQuaternion();
|
|
EXPECT_EQ(q, rq) << " 0 0 0 1 <-> SwingTwist";
|
|
|
|
q = Quaternion::Euler(90, 0, 0);
|
|
s = SwingTwist::FromQuaternion(q);
|
|
rq = s.ToQuaternion();
|
|
EXPECT_LT(Quaternion::Angle(q, rq), 10e-2) << " Euler 90 0 0 <-> SwingTwist";
|
|
|
|
q = Quaternion::Euler(0, 90, 0);
|
|
s = SwingTwist::FromQuaternion(q);
|
|
rq = s.ToQuaternion();
|
|
EXPECT_LT(Quaternion::Angle(q, rq), 10e-2) << " Euler 0 90 0 <-> SwingTwist";
|
|
|
|
q = Quaternion::Euler(0, 0, 90);
|
|
s = SwingTwist::FromQuaternion(q);
|
|
rq = s.ToQuaternion();
|
|
EXPECT_EQ(q, rq) << " Euler 0 0 90 <-> SwingTwist";
|
|
|
|
q = Quaternion::Euler(0, 180, 0); // ==> spherical S(180 0)T0
|
|
s = SwingTwist::FromQuaternion(q);
|
|
rq = s.ToQuaternion();
|
|
EXPECT_LT(Quaternion::Angle(q, rq), 10e-2) << " Euler 0 90 0 <-> SwingTwist";
|
|
|
|
q = Quaternion::Euler(0, 135, 0); // ==> spherical S(180 45)T0
|
|
s = SwingTwist::FromQuaternion(q);
|
|
rq = s.ToQuaternion();
|
|
EXPECT_LT(Quaternion::Angle(q, rq), 10e-2) << " Euler 0 90 0 <-> SwingTwist";
|
|
}
|
|
|
|
TYPED_TEST(SwingTwistTests, AngleAxis) {
|
|
using T = TypeParam;
|
|
using SwingTwist = SwingTwistOf<T>;
|
|
using Direction = DirectionOf<T>;
|
|
using Angle = AngleOf<T>;
|
|
|
|
SwingTwist s;
|
|
SwingTwist r;
|
|
|
|
s = SwingTwist::AngleAxis(0, Direction::up);
|
|
EXPECT_EQ(s, SwingTwist::Degrees(0, 0, 0)) << "0 up";
|
|
|
|
r = SwingTwist::AngleAxis(90, Direction::up);
|
|
s = SwingTwist::Degrees(90, 0, 0);
|
|
EXPECT_LT(SwingTwist::Angle(r, s), Angle::Degrees(10e-2f)) << "90 up";
|
|
|
|
r = SwingTwist::AngleAxis(180, Direction::up);
|
|
s = SwingTwist::Degrees(180, 0, 0);
|
|
EXPECT_LT(SwingTwist::Angle(r, s), Angle::Degrees(10e-2f)) << "180 up";
|
|
|
|
r = SwingTwist::AngleAxis(270, Direction::up);
|
|
s = SwingTwist::Degrees(-90, 0, 0);
|
|
EXPECT_LT(SwingTwist::Angle(r, s), Angle::Degrees(10e-2f)) << "270 up";
|
|
|
|
r = SwingTwist::AngleAxis(90, Direction::right);
|
|
s = SwingTwist::Degrees(0, 90, 0);
|
|
EXPECT_LT(SwingTwist::Angle(r, s), Angle::Degrees(10e-2f))
|
|
<< "90 right";
|
|
|
|
r = SwingTwist::AngleAxis(180, Direction::right);
|
|
s = SwingTwist::Degrees(0, 180, 0);
|
|
EXPECT_LT(SwingTwist::Angle(r, s), Angle::Degrees(10e-2f))
|
|
<< "180 right";
|
|
r = SwingTwist::AngleAxis(270, Direction::right);
|
|
s = SwingTwist::Degrees(0, -90, 0);
|
|
EXPECT_LT(SwingTwist::Angle(r, s), Angle::Degrees(10e-2f))
|
|
<< "270 right";
|
|
|
|
r = SwingTwist::AngleAxis(90, Direction::forward);
|
|
s = SwingTwist::Degrees(0, 0, 90);
|
|
EXPECT_LT(SwingTwist::Angle(r, s), Angle::Degrees(10e-2f)) << "90 up";
|
|
|
|
r = SwingTwist::AngleAxis(180, Direction::forward);
|
|
s = SwingTwist::Degrees(0, 0, 180);
|
|
EXPECT_LT(SwingTwist::Angle(r, s), Angle::Degrees(10e-2f)) << "180 up";
|
|
|
|
r = SwingTwist::AngleAxis(270, Direction::forward);
|
|
s = SwingTwist::Degrees(0, 0, -90);
|
|
EXPECT_LT(SwingTwist::Angle(r, s), Angle::Degrees(10e-2f)) << "270 up";
|
|
|
|
auto r16 = SwingTwist::AngleAxis(13, Direction::down);
|
|
auto s16 = SwingTwist::Degrees(-13, 0, 0);
|
|
EXPECT_LT(SwingTwist::Angle(r16, s16), Angle::Degrees(10e-2f))
|
|
<< "270 up";
|
|
}
|
|
|
|
TYPED_TEST(SwingTwistTests, Normalize) {
|
|
using T = TypeParam;
|
|
using SwingTwist = SwingTwistOf<T>;
|
|
|
|
SwingTwist s;
|
|
|
|
s = SwingTwist::Degrees(0, 0, 0);
|
|
EXPECT_EQ(s, SwingTwist::Degrees(0, 0, 0)) << "0 0 0 Normalized";
|
|
|
|
s = SwingTwist::Degrees(0, 180, 0);
|
|
EXPECT_EQ(s, SwingTwist::Degrees(180, 0, 180)) << "0 180 0 Normalized";
|
|
|
|
s = SwingTwist::Degrees(0, 180, 180);
|
|
EXPECT_EQ(s, SwingTwist::Degrees(180, 0, 0)) << "0 180 180 Normalized";
|
|
|
|
s = SwingTwist::Degrees(270, 90, 0);
|
|
EXPECT_EQ(s, SwingTwist::Degrees(-90, 90, 0)) << "270 90 0 Normalized";
|
|
|
|
s = SwingTwist::Degrees(270, 270, 0);
|
|
EXPECT_EQ(s, SwingTwist::Degrees(-90, -90, 0)) << "270 270 0 Normalized";
|
|
|
|
s = SwingTwist::Degrees(270, 225, 0);
|
|
EXPECT_EQ(s, SwingTwist::Degrees(90, -45, -180)) << "270 225 0 Normalized";
|
|
|
|
s = SwingTwist::Degrees(270, 0, 225);
|
|
EXPECT_EQ(s, SwingTwist::Degrees(-90, 0, -135)) << "270 0 225 Normalized";
|
|
}
|
|
|
|
#endif |