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

SwingTwist == and Angle

Browse Source
This commit is contained in:
Pascal Serrarens 2024-09-26 12:53:59 +02:00
parent 9406b57558
commit 0a07b23e96
7 changed files with 83 additions and 240 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
Angle.cpp
View File

@ -142,22 +142,22 @@ bool AngleOf<T>::operator==(const AngleOf<T> a) const {
} }
template <typename T> template <typename T>
bool AngleOf<T>::operator>(AngleOf<T> a) { bool AngleOf<T>::operator>(AngleOf<T> a) const {
return this->value > a.value; return this->value > a.value;
} }
template <typename T> template <typename T>
bool AngleOf<T>::operator>=(AngleOf<T> a) { bool AngleOf<T>::operator>=(AngleOf<T> a) const {
return this->value >= a.value; return this->value >= a.value;
} }
template <typename T> template <typename T>
bool AngleOf<T>::operator<(AngleOf<T> a) { bool AngleOf<T>::operator<(AngleOf<T> a) const {
return this->value < a.value; return this->value < a.value;
} }
template <typename T> template <typename T>
bool AngleOf<T>::operator<=(AngleOf<T> a) { bool AngleOf<T>::operator<=(AngleOf<T> a) const {
return this->value <= a.value; return this->value <= a.value;
} }

8
Angle.h
View File

@ -31,10 +31,10 @@ class AngleOf {
inline T GetBinary() const { return this->value; } inline T GetBinary() const { return this->value; }
bool operator==(const AngleOf<T> a) const; bool operator==(const AngleOf<T> a) const;
bool operator>(AngleOf<T> a); bool operator>(AngleOf<T> a) const;
bool operator>=(AngleOf<T> a); bool operator>=(AngleOf<T> a) const;
bool operator<(AngleOf<T> a); bool operator<(AngleOf<T> a) const;
bool operator<=(AngleOf<T> a); bool operator<=(AngleOf<T> a) const;
static signed int Sign(AngleOf<T> a); static signed int Sign(AngleOf<T> a);
static AngleOf<T> Abs(AngleOf<T> a); static AngleOf<T> Abs(AngleOf<T> a);

1
Direction.h
View File

@ -22,7 +22,6 @@ 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);
Vector3 ToVector3() const; Vector3 ToVector3() const;
static DirectionOf<T> FromVector3(Vector3 v); static DirectionOf<T> FromVector3(Vector3 v);

25
SwingTwist.cpp
View File

@ -11,8 +11,7 @@ SwingTwistOf<T>::SwingTwistOf() {
} }
template <typename T> template <typename T>
Passer::LinearAlgebra::SwingTwistOf<T>::SwingTwistOf(DirectionOf<T> swing, SwingTwistOf<T>::SwingTwistOf(DirectionOf<T> swing, AngleOf<T> twist) {
AngleOf<T> twist) {
this->swing = swing; this->swing = swing;
this->twist = twist; this->twist = twist;
} }
@ -46,8 +45,7 @@ Quaternion SwingTwistOf<T>::ToQuaternion() const {
} }
template <typename T> template <typename T>
SwingTwistOf<T> Passer::LinearAlgebra::SwingTwistOf<T>::FromQuaternion( SwingTwistOf<T> SwingTwistOf<T>::FromQuaternion(Quaternion q) {
Quaternion q) {
Vector3 angles = Quaternion::ToAngles(q); Vector3 angles = Quaternion::ToAngles(q);
SwingTwistOf<T> r = SwingTwistOf<T> r =
SwingTwistOf<T>::Degrees(angles.Up(), angles.Right(), angles.Forward()); SwingTwistOf<T>::Degrees(angles.Up(), angles.Right(), angles.Forward());
@ -56,7 +54,7 @@ SwingTwistOf<T> Passer::LinearAlgebra::SwingTwistOf<T>::FromQuaternion(
} }
template <typename T> template <typename T>
SphericalOf<T> Passer::LinearAlgebra::SwingTwistOf<T>::ToAngleAxis() const { SphericalOf<T> SwingTwistOf<T>::ToAngleAxis() const {
Quaternion q = this->ToQuaternion(); Quaternion q = this->ToQuaternion();
float angle; float angle;
Vector3 axis; Vector3 axis;
@ -68,13 +66,17 @@ SphericalOf<T> Passer::LinearAlgebra::SwingTwistOf<T>::ToAngleAxis() const {
} }
template <typename T> template <typename T>
SwingTwistOf<T> Passer::LinearAlgebra::SwingTwistOf<T>::FromAngleAxis( SwingTwistOf<T> SwingTwistOf<T>::FromAngleAxis(SphericalOf<T> aa) {
SphericalOf<T> aa) {
Vector3 vectorAxis = aa.direction.ToVector3(); Vector3 vectorAxis = aa.direction.ToVector3();
Quaternion q = Quaternion::AngleAxis(aa.distance, vectorAxis); Quaternion q = Quaternion::AngleAxis(aa.distance, vectorAxis);
return SwingTwistOf<T>(); return SwingTwistOf<T>();
} }
template <typename T>
bool SwingTwistOf<T>::operator==(const SwingTwistOf<T> s) const {
return (this->swing == s.swing) && (this->twist == s.twist);
}
template <typename T> template <typename T>
const SwingTwistOf<T> SwingTwistOf<T>::identity = SwingTwistOf(); const SwingTwistOf<T> SwingTwistOf<T>::identity = SwingTwistOf();
@ -120,5 +122,14 @@ SwingTwistOf<T> SwingTwistOf<T>::AngleAxis(float angle,
return r; return r;
} }
template <typename T>
AngleOf<T> SwingTwistOf<T>::Angle(const SwingTwistOf<T>& r1,
const SwingTwistOf<T>& r2) {
Quaternion q1 = r1.ToQuaternion();
Quaternion q2 = r2.ToQuaternion();
float angle = Quaternion::Angle(q1, q2);
return AngleOf<T>::Degrees(angle);
}
template class SwingTwistOf<float>; template class SwingTwistOf<float>;
template class SwingTwistOf<signed short>; template class SwingTwistOf<signed short>;

5
SwingTwist.h
View File

@ -26,6 +26,7 @@ class SwingTwistOf {
static SwingTwistOf<T> Degrees(float horizontal, static SwingTwistOf<T> Degrees(float horizontal,
float vertical = 0, float vertical = 0,
float twist = 0); float twist = 0);
Quaternion ToQuaternion() const; Quaternion ToQuaternion() const;
static SwingTwistOf<T> FromQuaternion(Quaternion q); static SwingTwistOf<T> FromQuaternion(Quaternion q);
@ -34,6 +35,8 @@ class SwingTwistOf {
const static SwingTwistOf<T> identity; const static SwingTwistOf<T> identity;
bool operator==(const SwingTwistOf<T> d) const;
/// <summary> /// <summary>
/// Rotate a vector using this rotation /// Rotate a vector using this rotation
/// </summary> /// </summary>
@ -59,6 +62,8 @@ class SwingTwistOf {
/// <param name="axis">The axis</param> /// <param name="axis">The axis</param>
/// <returns>The resulting quaternion</returns> /// <returns>The resulting quaternion</returns>
static SwingTwistOf<T> AngleAxis(float angle, const DirectionOf<T>& axis); static SwingTwistOf<T> AngleAxis(float angle, const DirectionOf<T>& axis);
static AngleOf<T> Angle(const SwingTwistOf<T>& r1, const SwingTwistOf<T>& r2);
}; };
using SwingTwistSingle = SwingTwistOf<float>; using SwingTwistSingle = SwingTwistOf<float>;

224
test/Angle_test.cc
View File

@ -1,224 +0,0 @@
/*
#if GTEST
#include <gtest/gtest.h>
#include <math.h>
#include <limits>
#include "Angle.h"
#define FLOAT_INFINITY std::numeric_limits<float>::infinity()
TEST(Angle16, Compare) {
Angle16 a = Angle16::Degrees(45);
bool r = false;
r = a > Angle16::Degrees(0);
EXPECT_TRUE(r) << "45 > 0";
r = a > Angle16::Degrees(90);
EXPECT_FALSE(r) << "45 > 90";
r = a > Angle16::Degrees(-90);
EXPECT_TRUE(r) << "45 > -90";
}
TEST(AngleSingle, Compare) {
AngleSingle a = AngleSingle::Degrees(45);
bool r = false;
r = a > AngleSingle::Degrees(0);
EXPECT_TRUE(r) << "45 > 0";
r = a > AngleSingle::Degrees(90);
EXPECT_FALSE(r) << "45 > 90";
r = a > AngleSingle::Degrees(-90);
EXPECT_TRUE(r) << "45 > -90";
}
TEST(Angle, Normalize) {
Angle r = AngleSingle();
r = Angle::Normalize(AngleSingle::Degrees(90.0f));
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Normalize 90";
r = Angle::Normalize(AngleSingle::Degrees(-90));
EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Normalize -90";
r = Angle::Normalize(AngleSingle::Degrees(270));
EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Normalize 270";
r = Angle::Normalize(AngleSingle::Degrees(270 + 360));
EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Normalize 270+360";
r = Angle::Normalize(AngleSingle::Degrees(-270));
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Normalize -270";
r = Angle::Normalize(AngleSingle::Degrees(-270 - 360));
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Normalize -270-360";
r = Angle::Normalize(AngleSingle::Degrees(0));
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Normalize 0";
if (std::numeric_limits<float>::is_iec559) {
r = Angle::Normalize(AngleSingle::Degrees(FLOAT_INFINITY));
EXPECT_FLOAT_EQ(r.InDegrees(), FLOAT_INFINITY) << "Normalize INFINITY";
r = Angle::Normalize(AngleSingle::Degrees(-FLOAT_INFINITY));
EXPECT_FLOAT_EQ(r.InDegrees(), -FLOAT_INFINITY) << "Normalize INFINITY";
}
}
TEST(Angle, Clamp) {
Angle r = AngleSingle();
r = Angle::Clamp(AngleSingle::Degrees(1), AngleSingle::Degrees(0),
AngleSingle::Degrees(2));
EXPECT_FLOAT_EQ(r.InDegrees(), 1) << "Clamp 1 0 2";
r = Angle::Clamp(AngleSingle::Degrees(-1), AngleSingle::Degrees(0),
AngleSingle::Degrees(2));
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Clamp -1 0 2";
r = Angle::Clamp(AngleSingle::Degrees(3), AngleSingle::Degrees(0),
AngleSingle::Degrees(2));
EXPECT_FLOAT_EQ(r.InDegrees(), 2) << "Clamp 3 0 2";
r = Angle::Clamp(AngleSingle::Degrees(1), AngleSingle::Degrees(0),
AngleSingle::Degrees(0));
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Clamp 1 0 0";
r = Angle::Clamp(AngleSingle::Degrees(0), AngleSingle::Degrees(0),
AngleSingle::Degrees(0));
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Clamp 0 0 0";
r = Angle::Clamp(AngleSingle::Degrees(0), AngleSingle::Degrees(1),
AngleSingle::Degrees(-1));
EXPECT_FLOAT_EQ(r.InDegrees(), 1) << "Clamp 0 1 -1";
if (std::numeric_limits<float>::is_iec559) {
r = Angle::Clamp(AngleSingle::Degrees(1), AngleSingle::Degrees(0),
AngleSingle::Degrees(FLOAT_INFINITY));
EXPECT_FLOAT_EQ(r.InDegrees(), 1) << "Clamp 1 0 INFINITY";
r = Angle::Clamp(AngleSingle::Degrees(1),
AngleSingle::Degrees(-FLOAT_INFINITY),
AngleSingle::Degrees(1));
EXPECT_FLOAT_EQ(r.InDegrees(), 1) << "Clamp 1 -INFINITY 1";
}
}
// TEST(Angle, Difference) {
// Angle r = 0;
// r = Angle::Difference(0, 90);
// EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Difference 0 90";
// r = Angle::Difference(0, -90);
// EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Difference 0 -90";
// r = Angle::Difference(0, 270);
// EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Difference 0 270";
// r = Angle::Difference(0, -270);
// EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Difference 0 -270";
// r = Angle::Difference(90, 0);
// EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Difference 90 0";
// r = Angle::Difference(-90, 0);
// EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Difference -90 0";
// r = Angle::Difference(0, 0);
// EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Difference 0 0";
// r = Angle::Difference(90, 90);
// EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Difference 90 90";
// if (std::numeric_limits<float>::is_iec559) {
// r = Angle::Difference(0, INFINITY);
// EXPECT_FLOAT_EQ(r.InDegrees(), INFINITY) << "Difference 0 INFINITY";
// r = Angle::Difference(0, -INFINITY);
// EXPECT_FLOAT_EQ(r.InDegrees(), -INFINITY) << "Difference 0 -INFINITY";
// r = Angle::Difference(-INFINITY, INFINITY);
// EXPECT_FLOAT_EQ(r.InDegrees(), INFINITY) << "Difference -INFINITY
// INFINITY";
// }
// }
TEST(Angle, MoveTowards) {
Angle r = Angle();
r = Angle::MoveTowards(Angle::Degrees(0), Angle::Degrees(90),
Angle::Degrees(30));
EXPECT_FLOAT_EQ(r.InDegrees(), 30) << "MoveTowards 0 90 30";
r = Angle::MoveTowards(Angle::Degrees(0), Angle::Degrees(90),
Angle::Degrees(90));
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "MoveTowards 0 90 90";
r = Angle::MoveTowards(Angle::Degrees(0), Angle::Degrees(90),
Angle::Degrees(180));
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "MoveTowards 0 90 180";
r = Angle::MoveTowards(Angle::Degrees(0), Angle::Degrees(90),
Angle::Degrees(270));
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "MoveTowards 0 90 270";
r = Angle::MoveTowards(Angle::Degrees(0), Angle::Degrees(90),
Angle::Degrees(-30));
EXPECT_FLOAT_EQ(r.InDegrees(), -30) << "MoveTowards 0 90 -30";
r = Angle::MoveTowards(Angle::Degrees(0), Angle::Degrees(-90),
Angle::Degrees(-30));
EXPECT_FLOAT_EQ(r.InDegrees(), 30) << "MoveTowards 0 -90 -30";
r = Angle::MoveTowards(Angle::Degrees(0), Angle::Degrees(-90),
Angle::Degrees(-90));
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "MoveTowards 0 -90 -90";
r = Angle::MoveTowards(Angle::Degrees(0), Angle::Degrees(-90),
Angle::Degrees(-180));
EXPECT_FLOAT_EQ(r.InDegrees(), 180) << "MoveTowards 0 -90 -180";
r = Angle::MoveTowards(Angle::Degrees(0), Angle::Degrees(-90),
Angle::Degrees(-270));
EXPECT_FLOAT_EQ(r.InDegrees(), 270) << "MoveTowards 0 -90 -270";
r = Angle::MoveTowards(Angle::Degrees(0), Angle::Degrees(90),
Angle::Degrees(0));
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 90 0";
r = Angle::MoveTowards(Angle::Degrees(0), Angle::Degrees(0),
Angle::Degrees(0));
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 0 0";
r = Angle::MoveTowards(Angle::Degrees(0), Angle::Degrees(0),
Angle::Degrees(30));
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 0 30";
if (std::numeric_limits<float>::is_iec559) {
r = Angle::MoveTowards(Angle::Degrees(0), Angle::Degrees(90),
Angle::Degrees(FLOAT_INFINITY));
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "MoveTowards 0 90 FLOAT_INFINITY";
r = Angle::MoveTowards(Angle::Degrees(0), Angle::Degrees(FLOAT_INFINITY),
Angle::Degrees(30));
EXPECT_FLOAT_EQ(r.InDegrees(), 30) << "MoveTowards 0 FLOAT_INFINITY 30";
r = Angle::MoveTowards(Angle::Degrees(0), Angle::Degrees(-90),
Angle::Degrees(-FLOAT_INFINITY));
EXPECT_FLOAT_EQ(r.InDegrees(), FLOAT_INFINITY)
<< "MoveTowards 0 -90 -FLOAT_INFINITY";
r = Angle::MoveTowards(Angle::Degrees(0), Angle::Degrees(-FLOAT_INFINITY),
Angle::Degrees(-30));
EXPECT_FLOAT_EQ(r.InDegrees(), 30) << "MoveTowards 0 -FLOAT_INFINITY -30";
}
}
#endif
*/

52
test/SwingTwistSingle_test.cc
View File

@ -43,4 +43,56 @@ TEST(SwingTwistSingle, Quaternion) {
EXPECT_LT(Quaternion::Angle(q, rq), 10e-2) << " Euler 0 90 0 <-> SwingTwist"; EXPECT_LT(Quaternion::Angle(q, rq), 10e-2) << " Euler 0 90 0 <-> SwingTwist";
} }
TEST(SwingTwistSingle, AngleAxis) {
SwingTwistSingle s;
SwingTwistSingle r;
s = SwingTwistSingle::AngleAxis(0, DirectionSingle::up);
EXPECT_EQ(s, SwingTwistSingle::Degrees(0, 0, 0)) << "0 up";
r = SwingTwistSingle::AngleAxis(90, DirectionSingle::up);
s = SwingTwistSingle::Degrees(90, 0, 0);
EXPECT_LT(SwingTwistSingle::Angle(r, s), AngleSingle::Degrees(10e-2f))
<< "90 up";
r = SwingTwistSingle::AngleAxis(180, DirectionSingle::up);
s = SwingTwistSingle::Degrees(180, 0, 0);
EXPECT_LT(SwingTwistSingle::Angle(r, s), AngleSingle::Degrees(10e-2f))
<< "180 up";
r = SwingTwistSingle::AngleAxis(270, DirectionSingle::up);
s = SwingTwistSingle::Degrees(-90, 0, 0);
EXPECT_LT(SwingTwistSingle::Angle(r, s), AngleSingle::Degrees(10e-2f))
<< "270 up";
r = SwingTwistSingle::AngleAxis(90, DirectionSingle::right);
s = SwingTwistSingle::Degrees(0, 90, 0);
EXPECT_LT(SwingTwistSingle::Angle(r, s), AngleSingle::Degrees(10e-2f))
<< "90 right";
r = SwingTwistSingle::AngleAxis(180, DirectionSingle::right);
s = SwingTwistSingle::Degrees(0, 180, 0);
EXPECT_LT(SwingTwistSingle::Angle(r, s), AngleSingle::Degrees(10e-2f))
<< "180 right";
r = SwingTwistSingle::AngleAxis(270, DirectionSingle::right);
s = SwingTwistSingle::Degrees(0, -90, 0);
EXPECT_LT(SwingTwistSingle::Angle(r, s), AngleSingle::Degrees(10e-2f))
<< "270 right";
r = SwingTwistSingle::AngleAxis(90, DirectionSingle::forward);
s = SwingTwistSingle::Degrees(0, 0, 90);
EXPECT_LT(SwingTwistSingle::Angle(r, s), AngleSingle::Degrees(10e-2f))
<< "90 up";
r = SwingTwistSingle::AngleAxis(180, DirectionSingle::forward);
s = SwingTwistSingle::Degrees(0, 0, 180);
EXPECT_LT(SwingTwistSingle::Angle(r, s), AngleSingle::Degrees(10e-2f))
<< "180 up";
r = SwingTwistSingle::AngleAxis(270, DirectionSingle::forward);
s = SwingTwistSingle::Degrees(0, 0, -90);
EXPECT_LT(SwingTwistSingle::Angle(r, s), AngleSingle::Degrees(10e-2f))
<< "270 up";
}
#endif #endif