46 lines
1.3 KiB
C++
46 lines
1.3 KiB
C++
#if GTEST
|
|
#include <gtest/gtest.h>
|
|
#include <math.h>
|
|
#include <limits>
|
|
|
|
#include "SwingTwist.h"
|
|
|
|
#define FLOAT_INFINITY std::numeric_limits<float>::infinity()
|
|
|
|
TEST(SwingTwistSingle, Quaternion) {
|
|
Quaternion q;
|
|
SwingTwistSingle s;
|
|
Quaternion rq;
|
|
|
|
q = Quaternion::identity;
|
|
s = SwingTwistSingle::FromQuaternion(q);
|
|
rq = s.ToQuaternion();
|
|
EXPECT_EQ(q, rq) << " 0 0 0 1 <-> SwingTwist";
|
|
|
|
q = Quaternion::Euler(90, 0, 0);
|
|
s = SwingTwistSingle::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 = SwingTwistSingle::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 = SwingTwistSingle::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 = SwingTwistSingle::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 = SwingTwistSingle::FromQuaternion(q);
|
|
rq = s.ToQuaternion();
|
|
EXPECT_LT(Quaternion::Angle(q, rq), 10e-2) << " Euler 0 90 0 <-> SwingTwist";
|
|
}
|
|
|
|
#endif |