RoboidControl/RoboidControl-cpp
RoboidControl/RoboidControl-cpp
1
0
Fork 0
Code Issues 1 Pull Requests Actions Packages Releases Activity

Extended unit tests (plus fixes)

Browse Source
This commit is contained in:
Pascal Serrarens 2024-09-26 10:10:28 +02:00
parent 8286c1ca85
commit 54d03185b4
13 changed files with 183 additions and 80 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
AngleAxis.cpp
View File

@ -19,7 +19,7 @@ AngleAxisOf<T>::AngleAxisOf(float angle, DirectionOf<T> axis) {
template <typename T> template <typename T>
AngleAxisOf<T>::AngleAxisOf(float angle, Vector3 axis) { AngleAxisOf<T>::AngleAxisOf(float angle, Vector3 axis) {
this->angle = angle; this->angle = angle;
this->axis = DirectionOf<T>(axis); this->axis = DirectionOf<T>::FromVector3(axis);
} }
template <typename T> template <typename T>
@ -28,7 +28,7 @@ AngleAxisOf<T>::AngleAxisOf(Quaternion q) {
Vector3 axis; Vector3 axis;
q.ToAngleAxis(&angle, &axis); q.ToAngleAxis(&angle, &axis);
this->angle = angle; this->angle = angle;
this->axis = DirectionOf<T>(axis); this->axis = DirectionOf<T>::FromVector3(axis);
} }
template <typename T> template <typename T>

13
CMakeLists.txt
View File

@ -40,21 +40,10 @@ else()
enable_testing() enable_testing()
file(GLOB_RECURSE test_srcs test/*.cc) file(GLOB_RECURSE test_srcs test/*_test.cc)
add_executable( add_executable(
LinearAlgebraTest LinearAlgebraTest
${test_srcs} ${test_srcs}
# "test/Angle_test.cc"
# "test/Direction_test.cc"
# "test/DiscreteAngle_test.cc"
# "test/FloatSingle_test.cc"
# "test/Matrix_test.cc"
# "test/Polar_test.cc"
# "test/Quaternion_test.cc"
# "test/Spherical_test.cc"
# "test/Spherical16_test.cc"
# "test/Vector2_test.cc"
# "test/Vector3_test.cc"
) )
target_link_libraries( target_link_libraries(
LinearAlgebraTest LinearAlgebraTest

44
Direction.cpp
View File

@ -22,17 +22,17 @@ DirectionOf<T>::DirectionOf(AngleOf<T> horizontal, AngleOf<T> vertical) {
Normalize(); Normalize();
}; };
template <typename T> // template <typename T>
DirectionOf<T>::DirectionOf(Vector3 v) { // DirectionOf<T>::DirectionOf(Vector3 v) {
this->horizontal = AngleOf<T>::Atan2( // this->horizontal = AngleOf<T>::Atan2(
v.Right(), // v.Right(),
v.Forward()); // AngleOf<T>::Radians(atan2f(v.Right(), v.Forward())); // v.Forward()); // AngleOf<T>::Radians(atan2f(v.Right(), v.Forward()));
this->vertical = // this->vertical =
-AngleOf<T>::deg90 - // -AngleOf<T>::deg90 -
AngleOf<T>::Acos( // AngleOf<T>::Acos(
v.Up()); // AngleOf<T>::Radians(-(0.5f * pi) - acosf(v.Up())); // v.Up()); // AngleOf<T>::Radians(-(0.5f * pi) - acosf(v.Up()));
Normalize(); // Normalize();
} // }
template <typename T> template <typename T>
const DirectionOf<T> DirectionOf<T>::forward = const DirectionOf<T> DirectionOf<T>::forward =
@ -53,6 +53,28 @@ template <typename T>
const DirectionOf<T> DirectionOf<T>::right = const DirectionOf<T> DirectionOf<T>::right =
DirectionOf<T>(AngleOf<T>::deg90, AngleOf<T>()); DirectionOf<T>(AngleOf<T>::deg90, AngleOf<T>());
template <typename T>
Vector3 Passer::LinearAlgebra::DirectionOf<T>::ToVector3() const {
Quaternion q = Quaternion::Euler(-this->vertical.InDegrees(),
this->horizontal.InDegrees(), 0);
Vector3 v = q * Vector3::forward;
return v;
}
template <typename T>
DirectionOf<T> Passer::LinearAlgebra::DirectionOf<T>::FromVector3(Vector3 v) {
DirectionOf<T> d;
d.horizontal = AngleOf<T>::Atan2(
v.Right(),
v.Forward()); // AngleOf<T>::Radians(atan2f(v.Right(), v.Forward()));
d.vertical =
-AngleOf<T>::deg90 -
AngleOf<T>::Acos(
v.Up()); // AngleOf<T>::Radians(-(0.5f * pi) - acosf(v.Up()));
d.Normalize();
return d;
}
template <typename T> template <typename T>
DirectionOf<T> Passer::LinearAlgebra::DirectionOf<T>::Degrees(float horizontal, DirectionOf<T> Passer::LinearAlgebra::DirectionOf<T>::Degrees(float horizontal,
float vertical) { float vertical) {

5
Direction.h
View File

@ -22,7 +22,10 @@ class DirectionOf {
DirectionOf<T>(); DirectionOf<T>();
DirectionOf<T>(AngleOf<T> horizontal, AngleOf<T> vertical); DirectionOf<T>(AngleOf<T> horizontal, AngleOf<T> vertical);
DirectionOf<T>(Vector3 v); // DirectionOf<T>(Vector3 v);
Vector3 ToVector3() const;
static DirectionOf<T> FromVector3(Vector3 v);
static DirectionOf<T> Degrees(float horizontal, float vertical); static DirectionOf<T> Degrees(float horizontal, float vertical);
static DirectionOf<T> Radians(float horizontal, float vertical); static DirectionOf<T> Radians(float horizontal, float vertical);

2
Quaternion.cpp
View File

@ -136,7 +136,7 @@ Vector3 Quaternion::operator*(const Vector3& p) const {
return result; return result;
} }
bool Quaternion::operator==(const Quaternion& q) { bool Quaternion::operator==(const Quaternion& q) const {
return (this->x == q.x && this->y == q.y && this->z == q.z && this->w == q.w); return (this->x == q.x && this->y == q.y && this->z == q.z && this->w == q.w);
} }

47
Quaternion.h
View File

@ -39,7 +39,7 @@ typedef struct Quat {
/// A quaternion /// A quaternion
/// </summary> /// </summary>
struct Quaternion : Quat { struct Quaternion : Quat {
public: public:
/// <summary> /// <summary>
/// Create a new identity quaternion /// Create a new identity quaternion
/// </summary> /// </summary>
@ -80,7 +80,7 @@ public:
/// <returns>A unit quaternion</returns> /// <returns>A unit quaternion</returns>
/// This will preserve the orientation, /// This will preserve the orientation,
/// but ensures that it is a unit quaternion. /// but ensures that it is a unit quaternion.
static Quaternion Normalize(const Quaternion &q); static Quaternion Normalize(const Quaternion& q);
/// <summary> /// <summary>
/// Convert to euler angles /// Convert to euler angles
@ -88,14 +88,14 @@ public:
/// <param name="q">The quaternion to convert</param> /// <param name="q">The quaternion to convert</param>
/// <returns>A vector containing euler angles</returns> /// <returns>A vector containing euler angles</returns>
/// The euler angles performed in the order: Z, X, Y /// The euler angles performed in the order: Z, X, Y
static Vector3 ToAngles(const Quaternion &q); static Vector3 ToAngles(const Quaternion& q);
/// <summary> /// <summary>
/// Rotate a vector using this quaterion /// Rotate a vector using this quaterion
/// </summary> /// </summary>
/// <param name="vector">The vector to rotate</param> /// <param name="vector">The vector to rotate</param>
/// <returns>The rotated vector</returns> /// <returns>The rotated vector</returns>
Vector3 operator*(const Vector3 &vector) const; Vector3 operator*(const Vector3& vector) const;
/// <summary> /// <summary>
/// Multiply this quaternion with another quaternion /// Multiply this quaternion with another quaternion
/// </summary> /// </summary>
@ -103,7 +103,7 @@ public:
/// <returns>The resulting rotation</returns> /// <returns>The resulting rotation</returns>
/// The result will be this quaternion rotated according to /// The result will be this quaternion rotated according to
/// the give rotation. /// the give rotation.
Quaternion operator*(const Quaternion &rotation) const; Quaternion operator*(const Quaternion& rotation) const;
/// <summary> /// <summary>
/// Check the equality of two quaternions /// Check the equality of two quaternions
@ -114,7 +114,7 @@ public:
/// themselves. Two quaternions with the same rotational effect may have /// themselves. Two quaternions with the same rotational effect may have
/// different components. Use Quaternion::Angle to check if the rotations are /// different components. Use Quaternion::Angle to check if the rotations are
/// the same. /// the same.
bool operator==(const Quaternion &quaternion); bool operator==(const Quaternion& quaternion) const;
/// <summary> /// <summary>
/// The inverse of quaterion /// The inverse of quaterion
@ -129,8 +129,8 @@ public:
/// <param name="forward">The look direction</param> /// <param name="forward">The look direction</param>
/// <param name="upwards">The up direction</param> /// <param name="upwards">The up direction</param>
/// <returns>The look rotation</returns> /// <returns>The look rotation</returns>
static Quaternion LookRotation(const Vector3 &forward, static Quaternion LookRotation(const Vector3& forward,
const Vector3 &upwards); const Vector3& upwards);
/// <summary> /// <summary>
/// Creates a quaternion with the given forward direction with up = /// Creates a quaternion with the given forward direction with up =
/// Vector3::up /// Vector3::up
@ -140,7 +140,7 @@ public:
/// For the rotation, Vector::up is used for the up direction. /// For the rotation, Vector::up is used for the up direction.
/// Note: if the forward direction == Vector3::up, the result is /// Note: if the forward direction == Vector3::up, the result is
/// Quaternion::identity /// Quaternion::identity
static Quaternion LookRotation(const Vector3 &forward); static Quaternion LookRotation(const Vector3& forward);
/// <summary> /// <summary>
/// Calculat the rotation from on vector to another /// Calculat the rotation from on vector to another
@ -157,7 +157,8 @@ public:
/// <param name="to">The destination rotation</param> /// <param name="to">The destination rotation</param>
/// <param name="maxDegreesDelta">The maximum amount of degrees to /// <param name="maxDegreesDelta">The maximum amount of degrees to
/// rotate</param> <returns>The possibly limited rotation</returns> /// rotate</param> <returns>The possibly limited rotation</returns>
static Quaternion RotateTowards(const Quaternion &from, const Quaternion &to, static Quaternion RotateTowards(const Quaternion& from,
const Quaternion& to,
float maxDegreesDelta); float maxDegreesDelta);
/// <summary> /// <summary>
@ -166,13 +167,13 @@ public:
/// <param name="angle">The angle</param> /// <param name="angle">The angle</param>
/// <param name="axis">The axis</param> /// <param name="axis">The axis</param>
/// <returns>The resulting quaternion</returns> /// <returns>The resulting quaternion</returns>
static Quaternion AngleAxis(float angle, const Vector3 &axis); static Quaternion AngleAxis(float angle, const Vector3& axis);
/// <summary> /// <summary>
/// Convert this quaternion to angle/axis representation /// Convert this quaternion to angle/axis representation
/// </summary> /// </summary>
/// <param name="angle">A pointer to the angle for the result</param> /// <param name="angle">A pointer to the angle for the result</param>
/// <param name="axis">A pointer to the axis for the result</param> /// <param name="axis">A pointer to the axis for the result</param>
void ToAngleAxis(float *angle, Vector3 *axis); void ToAngleAxis(float* angle, Vector3* axis);
/// <summary> /// <summary>
/// Get the angle between two orientations /// Get the angle between two orientations
@ -190,8 +191,9 @@ public:
/// <param name="factor">The factor between 0 and 1.</param> /// <param name="factor">The factor between 0 and 1.</param>
/// <returns>The resulting rotation</returns> /// <returns>The resulting rotation</returns>
/// A factor 0 returns rotation1, factor1 returns rotation2. /// A factor 0 returns rotation1, factor1 returns rotation2.
static Quaternion Slerp(const Quaternion &rotation1, static Quaternion Slerp(const Quaternion& rotation1,
const Quaternion &rotation2, float factor); const Quaternion& rotation2,
float factor);
/// <summary> /// <summary>
/// Unclamped sherical lerp between two rotations /// Unclamped sherical lerp between two rotations
/// </summary> /// </summary>
@ -201,8 +203,9 @@ public:
/// <returns>The resulting rotation</returns> /// <returns>The resulting rotation</returns>
/// A factor 0 returns rotation1, factor1 returns rotation2. /// A factor 0 returns rotation1, factor1 returns rotation2.
/// Values outside the 0..1 range will result in extrapolated rotations /// Values outside the 0..1 range will result in extrapolated rotations
static Quaternion SlerpUnclamped(const Quaternion &rotation1, static Quaternion SlerpUnclamped(const Quaternion& rotation1,
const Quaternion &rotation2, float factor); const Quaternion& rotation2,
float factor);
/// <summary> /// <summary>
/// Create a rotation from euler angles /// Create a rotation from euler angles
@ -260,8 +263,10 @@ public:
/// <param name="swing">A pointer to the quaternion for the swing /// <param name="swing">A pointer to the quaternion for the swing
/// result</param> <param name="twist">A pointer to the quaternion for the /// result</param> <param name="twist">A pointer to the quaternion for the
/// twist result</param> /// twist result</param>
static void GetSwingTwist(Vector3 axis, Quaternion rotation, static void GetSwingTwist(Vector3 axis,
Quaternion *swing, Quaternion *twist); Quaternion rotation,
Quaternion* swing,
Quaternion* twist);
/// <summary> /// <summary>
/// Calculate the dot product of two quaternions /// Calculate the dot product of two quaternions
@ -271,12 +276,12 @@ public:
/// <returns></returns> /// <returns></returns>
static float Dot(Quaternion rotation1, Quaternion rotation2); static float Dot(Quaternion rotation1, Quaternion rotation2);
private: private:
float GetLength() const; float GetLength() const;
float GetLengthSquared() const; float GetLengthSquared() const;
static float GetLengthSquared(const Quaternion &q); static float GetLengthSquared(const Quaternion& q);
void ToAxisAngleRad(const Quaternion &q, Vector3 *const axis, float *angle); void ToAxisAngleRad(const Quaternion& q, Vector3* const axis, float* angle);
static Quaternion FromEulerRad(Vector3 euler); static Quaternion FromEulerRad(Vector3 euler);
static Quaternion FromEulerRadXYZ(Vector3 euler); static Quaternion FromEulerRadXYZ(Vector3 euler);

20
SwingTwist.cpp
View File

@ -55,6 +55,26 @@ SwingTwistOf<T> Passer::LinearAlgebra::SwingTwistOf<T>::FromQuaternion(
return r; return r;
} }
template <typename T>
SphericalOf<T> Passer::LinearAlgebra::SwingTwistOf<T>::ToAngleAxis() const {
Quaternion q = this->ToQuaternion();
float angle;
Vector3 axis;
q.ToAngleAxis(&angle, &axis);
DirectionOf<T> direction = DirectionOf<T>::FromVector3(axis);
SphericalOf<T> aa = SphericalOf<T>(angle, direction);
return aa;
}
template <typename T>
SwingTwistOf<T> Passer::LinearAlgebra::SwingTwistOf<T>::FromAngleAxis(
SphericalOf<T> aa) {
Vector3 vectorAxis = aa.direction.ToVector3();
Quaternion q = Quaternion::AngleAxis(aa.distance, vectorAxis);
return SwingTwistOf<T>();
}
template <typename T> template <typename T>
const SwingTwistOf<T> SwingTwistOf<T>::identity = SwingTwistOf(); const SwingTwistOf<T> SwingTwistOf<T>::identity = SwingTwistOf();

3
SwingTwist.h
View File

@ -29,6 +29,9 @@ class SwingTwistOf {
Quaternion ToQuaternion() const; Quaternion ToQuaternion() const;
static SwingTwistOf<T> FromQuaternion(Quaternion q); static SwingTwistOf<T> FromQuaternion(Quaternion q);
SphericalOf<T> ToAngleAxis() const;
static SwingTwistOf<T> FromAngleAxis(SphericalOf<T> aa);
const static SwingTwistOf<T> identity; const static SwingTwistOf<T> identity;
/// <summary> /// <summary>

2
Vector3.cpp
View File

@ -153,7 +153,7 @@ float Vector3::Dot(const Vector3& v1, const Vector3& v2) {
return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z; return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z;
} }
bool Vector3::operator==(const Vector3& v) { bool Vector3::operator==(const Vector3& v) const {
return (this->x == v.x && this->y == v.y && this->z == v.z); return (this->x == v.x && this->y == v.y && this->z == v.z);
} }

2
Vector3.h
View File

@ -90,7 +90,7 @@ struct Vector3 : Vec3 {
/// @return true if it is identical to the given vector /// @return true if it is identical to the given vector
/// @note This uses float comparison to check equality which may have strange /// @note This uses float comparison to check equality which may have strange
/// effects. Equality on floats should be avoided. /// effects. Equality on floats should be avoided.
bool operator==(const Vector3& v); bool operator==(const Vector3& v) const;
/// @brief The vector length /// @brief The vector length
/// @param v The vector for which you need the length /// @param v The vector for which you need the length

12
test/Spherical16_test.cc
View File

@ -33,6 +33,18 @@ TEST(Spherical16, FromVector3) {
EXPECT_FLOAT_EQ(s.direction.vertical.InDegrees(), 0.0F) << "s.vert 1 0 0"; EXPECT_FLOAT_EQ(s.direction.vertical.InDegrees(), 0.0F) << "s.vert 1 0 0";
} }
TEST(Spherical16, Vector3) {
Vector3 v = Vector3(1, 2, 3);
Spherical16 rd = Spherical16::FromVector3(v);
Vector3 rv = rd.ToVector3();
EXPECT_LT(Vector3::Distance(v, rv), 10e-4) << " 1 2 3 <-> spherical";
v = Vector3(1, 2, -3);
rd = Spherical16::FromVector3(v);
rv = rd.ToVector3();
EXPECT_LT(Vector3::Distance(v, rv), 10e-4) << " 1 2 3 <-> spherical";
}
// TEST(Spherical16, FromPolar) { // TEST(Spherical16, FromPolar) {
// Polar p = Polar(1, 0); // Polar p = Polar(1, 0);
// Spherical16 s = Spherical16::FromPolar(p); // Spherical16 s = Spherical16::FromPolar(p);

59
test/Spherical_test.cc → test/SphericalSingle_test.cc
View File

@ -7,32 +7,32 @@
#define FLOAT_INFINITY std::numeric_limits<float>::infinity() #define FLOAT_INFINITY std::numeric_limits<float>::infinity()
TEST(Spherical, FromVector3) { TEST(SphericalSingle, FromVector3) {
Vector3 v = Vector3(0, 0, 1); Vector3 v = Vector3(0, 0, 1);
Spherical s = Spherical::FromVector3(v); SphericalSingle s = SphericalSingle ::FromVector3(v);
EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance 0 0 1"; EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance 0 0 1";
EXPECT_FLOAT_EQ(s.direction.horizontal.InDegrees(), 0.0F) << "s.hor 0 0 1"; EXPECT_FLOAT_EQ(s.direction.horizontal.InDegrees(), 0.0F) << "s.hor 0 0 1";
EXPECT_FLOAT_EQ(s.direction.vertical.InDegrees(), 0.0F) << "s.vert 0 0 1"; EXPECT_FLOAT_EQ(s.direction.vertical.InDegrees(), 0.0F) << "s.vert 0 0 1";
v = Vector3(0, 1, 0); v = Vector3(0, 1, 0);
s = Spherical::FromVector3(v); s = SphericalSingle ::FromVector3(v);
EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance 0 1 0"; EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance 0 1 0";
EXPECT_FLOAT_EQ(s.direction.horizontal.InDegrees(), 0.0F) << "s.hor 0 1 0"; EXPECT_FLOAT_EQ(s.direction.horizontal.InDegrees(), 0.0F) << "s.hor 0 1 0";
EXPECT_FLOAT_EQ(s.direction.vertical.InDegrees(), 90.0F) << "s.vert 0 1 0"; EXPECT_FLOAT_EQ(s.direction.vertical.InDegrees(), 90.0F) << "s.vert 0 1 0";
v = Vector3(1, 0, 0); v = Vector3(1, 0, 0);
s = Spherical::FromVector3(v); s = SphericalSingle ::FromVector3(v);
EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance 1 0 0"; EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance 1 0 0";
EXPECT_FLOAT_EQ(s.direction.horizontal.InDegrees(), 90.0F) << "s.hor 1 0 0"; EXPECT_FLOAT_EQ(s.direction.horizontal.InDegrees(), 90.0F) << "s.hor 1 0 0";
EXPECT_FLOAT_EQ(s.direction.vertical.InDegrees(), 0.0F) << "s.vert 1 0 0"; EXPECT_FLOAT_EQ(s.direction.vertical.InDegrees(), 0.0F) << "s.vert 1 0 0";
} }
TEST(Spherical, FromPolar) { TEST(SphericalSingle, FromPolar) {
Polar p = Polar(1, Angle::Degrees(0)); Polar p = Polar(1, Angle::Degrees(0));
Spherical s = Spherical::FromPolar(p); SphericalSingle s = SphericalSingle ::FromPolar(p);
EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance Polar(1 0)"; EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance Polar(1 0)";
EXPECT_FLOAT_EQ(s.direction.horizontal.InDegrees(), 0.0F) EXPECT_FLOAT_EQ(s.direction.horizontal.InDegrees(), 0.0F)
@ -41,7 +41,7 @@ TEST(Spherical, FromPolar) {
<< "s.vert Polar(1 0)"; << "s.vert Polar(1 0)";
p = Polar(1, Angle::Degrees(45)); p = Polar(1, Angle::Degrees(45));
s = Spherical::FromPolar(p); s = SphericalSingle ::FromPolar(p);
EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance Polar(1 45)"; EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance Polar(1 45)";
EXPECT_FLOAT_EQ(s.direction.horizontal.InDegrees(), 45.0F) EXPECT_FLOAT_EQ(s.direction.horizontal.InDegrees(), 45.0F)
@ -50,7 +50,7 @@ TEST(Spherical, FromPolar) {
<< "s.vert Polar(1 45)"; << "s.vert Polar(1 45)";
p = Polar(1, Angle::Degrees(-45)); p = Polar(1, Angle::Degrees(-45));
s = Spherical::FromPolar(p); s = SphericalSingle ::FromPolar(p);
EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance Polar(1 -45)"; EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance Polar(1 -45)";
EXPECT_FLOAT_EQ(s.direction.horizontal.InDegrees(), -45.0F) EXPECT_FLOAT_EQ(s.direction.horizontal.InDegrees(), -45.0F)
@ -59,7 +59,7 @@ TEST(Spherical, FromPolar) {
<< "s.vert Polar(1 -45)"; << "s.vert Polar(1 -45)";
p = Polar(0, Angle::Degrees(0)); p = Polar(0, Angle::Degrees(0));
s = Spherical::FromPolar(p); s = SphericalSingle ::FromPolar(p);
EXPECT_FLOAT_EQ(s.distance, 0.0F) << "s.distance Polar(0 0)"; EXPECT_FLOAT_EQ(s.distance, 0.0F) << "s.distance Polar(0 0)";
EXPECT_FLOAT_EQ(s.direction.horizontal.InDegrees(), 0.0F) EXPECT_FLOAT_EQ(s.direction.horizontal.InDegrees(), 0.0F)
@ -68,7 +68,7 @@ TEST(Spherical, FromPolar) {
<< "s.vert Polar(0 0)"; << "s.vert Polar(0 0)";
p = Polar(-1, Angle::Degrees(0)); p = Polar(-1, Angle::Degrees(0));
s = Spherical::FromPolar(p); s = SphericalSingle ::FromPolar(p);
EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance Polar(-1 0)"; EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance Polar(-1 0)";
EXPECT_FLOAT_EQ(s.direction.horizontal.InDegrees(), -180.0F) EXPECT_FLOAT_EQ(s.direction.horizontal.InDegrees(), -180.0F)
@ -77,12 +77,12 @@ TEST(Spherical, FromPolar) {
<< "s.vert Polar(-1 0)"; << "s.vert Polar(-1 0)";
} }
TEST(Spherical, Incident1) { TEST(SphericalSingle, Incident1) {
Vector3 v = Vector3(2.242557f, 1.027884f, -0.322347f); Vector3 v = Vector3(2.242557f, 1.027884f, -0.322347f);
Spherical s = Spherical::FromVector3(v); SphericalSingle s = SphericalSingle ::FromVector3(v);
Spherical sr = SphericalSingle sr =
Spherical(2.49F, Angle::Degrees(98.18f), Angle::Degrees(24.4F)); SphericalSingle(2.49F, Angle::Degrees(98.18f), Angle::Degrees(24.4F));
EXPECT_NEAR(s.distance, sr.distance, 1.0e-01); EXPECT_NEAR(s.distance, sr.distance, 1.0e-01);
EXPECT_NEAR(s.direction.horizontal.InDegrees(), EXPECT_NEAR(s.direction.horizontal.InDegrees(),
sr.direction.horizontal.InDegrees(), 1.0e-02); sr.direction.horizontal.InDegrees(), 1.0e-02);
@ -95,12 +95,12 @@ TEST(Spherical, Incident1) {
EXPECT_NEAR(r.Forward(), v.Forward(), 1.0e-02) << "toVector3.z 1 0 0"; EXPECT_NEAR(r.Forward(), v.Forward(), 1.0e-02) << "toVector3.z 1 0 0";
} }
TEST(Spherical, Incident2) { TEST(SphericalSingle, Incident2) {
Vector3 v = Vector3(1.0f, 0.0f, 1.0f); Vector3 v = Vector3(1.0f, 0.0f, 1.0f);
Spherical s = Spherical::FromVector3(v); SphericalSingle s = SphericalSingle ::FromVector3(v);
Spherical sr = SphericalSingle sr = SphericalSingle(1.4142135623F, Angle::Degrees(45.0f),
Spherical(1.4142135623F, Angle::Degrees(45.0f), Angle::Degrees(0.0F)); Angle::Degrees(0.0F));
EXPECT_NEAR(s.distance, sr.distance, 1.0e-05); EXPECT_NEAR(s.distance, sr.distance, 1.0e-05);
EXPECT_NEAR(s.direction.horizontal.InDegrees(), EXPECT_NEAR(s.direction.horizontal.InDegrees(),
sr.direction.horizontal.InDegrees(), 1.0e-05); sr.direction.horizontal.InDegrees(), 1.0e-05);
@ -113,9 +113,10 @@ TEST(Spherical, Incident2) {
EXPECT_NEAR(r.Forward(), v.Forward(), 1.0e-06); EXPECT_NEAR(r.Forward(), v.Forward(), 1.0e-06);
v = Vector3(0.0f, 1.0f, 1.0f); v = Vector3(0.0f, 1.0f, 1.0f);
s = Spherical::FromVector3(v); s = SphericalSingle ::FromVector3(v);
sr = Spherical(1.4142135623F, Angle::Degrees(0.0f), Angle::Degrees(45.0F)); sr = SphericalSingle(1.4142135623F, Angle::Degrees(0.0f),
Angle::Degrees(45.0F));
EXPECT_NEAR(s.distance, sr.distance, 1.0e-05); EXPECT_NEAR(s.distance, sr.distance, 1.0e-05);
EXPECT_NEAR(s.direction.horizontal.InDegrees(), EXPECT_NEAR(s.direction.horizontal.InDegrees(),
sr.direction.horizontal.InDegrees(), 1.0e-05); sr.direction.horizontal.InDegrees(), 1.0e-05);
@ -128,7 +129,7 @@ TEST(Spherical, Incident2) {
EXPECT_NEAR(r.Forward(), v.Forward(), 1.0e-06); EXPECT_NEAR(r.Forward(), v.Forward(), 1.0e-06);
v = Vector3(1.0f, 1.0f, 1.0f); v = Vector3(1.0f, 1.0f, 1.0f);
s = Spherical::FromVector3(v); s = SphericalSingle ::FromVector3(v);
r = Vector3(s); r = Vector3(s);
EXPECT_NEAR(s.distance, 1.73205080F, 1.0e-02); EXPECT_NEAR(s.distance, 1.73205080F, 1.0e-02);
@ -139,17 +140,19 @@ TEST(Spherical, Incident2) {
EXPECT_NEAR(r.Up(), v.Up(), 1.0e-06); EXPECT_NEAR(r.Up(), v.Up(), 1.0e-06);
EXPECT_NEAR(r.Forward(), v.Forward(), 1.0e-06); EXPECT_NEAR(r.Forward(), v.Forward(), 1.0e-06);
// s = Spherical(10, 45, 45); // s = SphericalSingle
(10, 45, 45);
// r = s.ToVector3(); // r = s.ToVector3();
// EXPECT_NEAR(r.x, 5, 1.0e-06); // EXPECT_NEAR(r.x, 5, 1.0e-06);
// EXPECT_NEAR(r.y, 7.07, 1.0e-06); // EXPECT_NEAR(r.y, 7.07, 1.0e-06);
// EXPECT_NEAR(r.z, 5, 1.0e-06); // EXPECT_NEAR(r.z, 5, 1.0e-06);
} }
TEST(Spherical, Addition) { TEST(SphericalSingle, Addition) {
Spherical v1 = Spherical(1, Angle::Degrees(45), Angle::Degrees(0)); SphericalSingle v1 =
Spherical v2 = Spherical::zero; SphericalSingle(1, Angle::Degrees(45), Angle::Degrees(0));
Spherical r = Spherical::zero; SphericalSingle v2 = SphericalSingle ::zero;
SphericalSingle r = SphericalSingle ::zero;
r = v1 + v2; r = v1 + v2;
EXPECT_FLOAT_EQ(r.distance, v1.distance) << "Addition(0 0 0)"; EXPECT_FLOAT_EQ(r.distance, v1.distance) << "Addition(0 0 0)";
@ -158,13 +161,13 @@ TEST(Spherical, Addition) {
r += v2; r += v2;
EXPECT_FLOAT_EQ(r.distance, v1.distance) << "Addition(0 0 0)"; EXPECT_FLOAT_EQ(r.distance, v1.distance) << "Addition(0 0 0)";
v2 = Spherical(1, Angle::Degrees(-45), Angle::Degrees(0)); v2 = SphericalSingle(1, Angle::Degrees(-45), Angle::Degrees(0));
r = v1 + v2; r = v1 + v2;
EXPECT_FLOAT_EQ(r.distance, sqrtf(2)) << "Addition(1 -45 0)"; EXPECT_FLOAT_EQ(r.distance, sqrtf(2)) << "Addition(1 -45 0)";
EXPECT_FLOAT_EQ(r.direction.horizontal.InDegrees(), 0) << "Addition(1 -45 0)"; EXPECT_FLOAT_EQ(r.direction.horizontal.InDegrees(), 0) << "Addition(1 -45 0)";
EXPECT_FLOAT_EQ(r.direction.vertical.InDegrees(), 0) << "Addition(1 -45 0)"; EXPECT_FLOAT_EQ(r.direction.vertical.InDegrees(), 0) << "Addition(1 -45 0)";
v2 = Spherical(1, Angle::Degrees(0), Angle::Degrees(90)); v2 = SphericalSingle(1, Angle::Degrees(0), Angle::Degrees(90));
r = v1 + v2; r = v1 + v2;
EXPECT_FLOAT_EQ(r.distance, sqrtf(2)) << "Addition(1 0 90)"; EXPECT_FLOAT_EQ(r.distance, sqrtf(2)) << "Addition(1 0 90)";
EXPECT_FLOAT_EQ(r.direction.horizontal.InDegrees(), 45) << "Addition(1 0 90)"; EXPECT_FLOAT_EQ(r.direction.horizontal.InDegrees(), 45) << "Addition(1 0 90)";

46
test/SwingTwistSingle_test.cc Normal file
View File

@ -0,0 +1,46 @@
#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