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

Normalizing directions

Browse Source
This commit is contained in:
Pascal Serrarens 2024-09-23 14:20:07 +02:00
parent 1ea65d56b1
commit 92d5ef028c
7 changed files with 116 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Angle.cpp
View File

@ -112,7 +112,7 @@ float AngleOf<T>::InRadians() const {
// AngleOf<float> AngleOf<float>::Deg2Rad = (pi * 2) / 360.0f;
template <typename T>
bool AngleOf<T>::operator==(AngleOf<T> a) {
bool AngleOf<T>::operator==(const AngleOf<T> a) const {
return this->value == a.value;
}

2
Angle.h
View File

@ -33,7 +33,7 @@ class AngleOf {
// static AngleOf<T> pi;
bool operator==(AngleOf<T> a);
bool operator==(const AngleOf<T> a) const;
bool operator>(AngleOf<T> a);
bool operator>=(AngleOf<T> a);
bool operator<(AngleOf<T> a);

23
CMakeLists.txt
View File

@ -43,18 +43,21 @@ else()
enable_testing()
file(GLOB_RECURSE test_srcs test/*.cc)
add_executable(
LinearAlgebraTest
"test/Angle_test.cc"
"test/FloatSingle_test.cc"
"test/Vector2_test.cc"
"test/Vector3_test.cc"
"test/Quaternion_test.cc"
"test/Matrix_test.cc"
"test/Polar_test.cc"
"test/Spherical_test.cc"
"test/Spherical16_test.cc"
"test/DiscreteAngle_test.cc"
${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(
LinearAlgebraTest

50
Direction.cpp
View File

@ -19,6 +19,7 @@ template <typename T>
DirectionOf<T>::DirectionOf(AngleOf<T> horizontal, AngleOf<T> vertical) {
this->horizontal = horizontal;
this->vertical = vertical;
Normalize();
};
template <typename T>
@ -26,6 +27,7 @@ DirectionOf<T>::DirectionOf(Vector3 v) {
this->horizontal =
atan2f(v.Right(), v.Forward()) * Passer::LinearAlgebra::Rad2Deg;
this->vertical = 90 - acosf(v.Up()) * Passer::LinearAlgebra::Rad2Deg;
Normalize();
}
template <typename T>
@ -41,28 +43,33 @@ const DirectionOf<T> DirectionOf<T>::left = DirectionOf<T>(-90.0f, 0.0f);
template <typename T>
const DirectionOf<T> DirectionOf<T>::right = DirectionOf<T>(90.0f, 0.0f);
template <typename T>
DirectionOf<T> Passer::LinearAlgebra::DirectionOf<T>::Degrees(float horizontal,
float vertical) {
return DirectionOf<T>(AngleOf<T>::Degrees(horizontal),
AngleOf<T>::Degrees(vertical));
}
template <typename T>
DirectionOf<T> Passer::LinearAlgebra::DirectionOf<T>::Radians(float horizontal,
float vertical) {
return DirectionOf<T>(AngleOf<T>::Radians(horizontal),
AngleOf<T>::Radians(vertical));
}
template <typename T>
bool Passer::LinearAlgebra::DirectionOf<T>::operator==(
const DirectionOf<T> d) const {
return (this->horizontal == d.horizontal) && (this->vertical == d.vertical);
}
template <typename T>
DirectionOf<T> Passer::LinearAlgebra::DirectionOf<T>::operator-() const {
DirectionOf<T> r = DirectionOf<T>(-this->horizontal, -this->vertical);
DirectionOf<T> r = DirectionOf<T>(this->horizontal + AngleOf<T>::Degrees(180),
-this->vertical);
return r;
}
template <typename T>
DirectionOf<T> Passer::LinearAlgebra::DirectionOf<T>::operator+(
const DirectionOf<T>& v) const {
DirectionOf<T> r = DirectionOf<T>(this->horizontal + v.horizontal,
this->vertical + v.vertical);
return r;
}
template <typename T>
DirectionOf<T> Passer::LinearAlgebra::DirectionOf<T>::operator+=(
const DirectionOf<T>& v) {
this->horizontal += v.horizontal;
this->vertical += v.vertical;
return *this;
}
template <typename T>
Vector3 DirectionOf<T>::ToVector3() {
Vector3 v = Quaternion::Euler(-(this->vertical.InDegrees()),
@ -71,5 +78,14 @@ Vector3 DirectionOf<T>::ToVector3() {
return v;
}
template <typename T>
void DirectionOf<T>::Normalize() {
if (this->vertical > AngleOf<T>::Degrees(90) ||
this->vertical < AngleOf<T>::Degrees(-90)) {
this->horizontal += AngleOf<T>::Degrees(180);
this->vertical = AngleOf<T>::Degrees(180) - this->vertical;
}
}
template class DirectionOf<float>;
template class DirectionOf<signed short>;

12
Direction.h
View File

@ -15,13 +15,18 @@ struct Vector3;
template <typename T>
class DirectionOf {
public:
/// @brief horizontal angle, range= (-180..180]
AngleOf<T> horizontal;
/// @brief vertical angle, range in degrees = (-90..90]
AngleOf<T> vertical;
DirectionOf<T>();
DirectionOf<T>(AngleOf<T> horizontal, AngleOf<T> vertical);
DirectionOf<T>(Vector3 v);
static DirectionOf<T> Degrees(float horizontal, float vertical);
static DirectionOf<T> Radians(float horizontal, float vertical);
const static DirectionOf forward;
const static DirectionOf back;
const static DirectionOf up;
@ -29,11 +34,14 @@ class DirectionOf {
const static DirectionOf left;
const static DirectionOf right;
bool operator==(const DirectionOf<T> d) const;
DirectionOf<T> operator-() const;
DirectionOf<T> operator+(const DirectionOf<T>& v) const;
DirectionOf<T> operator+=(const DirectionOf<T>& v);
Vector3 ToVector3();
protected:
void Normalize();
};
using DirectionSingle = DirectionOf<float>;

5
SwingTwist.cpp
View File

@ -79,9 +79,8 @@ SwingTwistOf<T> SwingTwistOf<T>::operator*(
template <typename T>
SwingTwistOf<T> SwingTwistOf<T>::operator*=(const SwingTwistOf<T>& rotation) {
// this->swing.horizontal += rotation.swing.horizontal;
// this->swing.vertical += rotation.swing.vertical;
this->swing += rotation.swing;
this->swing.horizontal += rotation.swing.horizontal;
this->swing.vertical += rotation.swing.vertical;
this->twist += rotation.twist;
return *this;
}

56
test/Direction_test.cc Normal file
View File

@ -0,0 +1,56 @@
#if GTEST
#include <gtest/gtest.h>
#include <math.h>
#include <limits>
#include "Direction.h"
#define FLOAT_INFINITY std::numeric_limits<float>::infinity()
TEST(Direction16, Compare) {
Direction16 d = Direction16::Degrees(45, 135);
bool r;
r = (d == Direction16(Angle16::Degrees(45), Angle16::Degrees(135)));
EXPECT_TRUE(r) << "45,135 == 45, 135";
r = (d ==
Direction16(Angle16::Degrees(45 + 360), Angle16::Degrees(135 - 360)));
EXPECT_TRUE(r) << "45+360, 135-360 == 45, 135";
}
TEST(Direction16, Inverse) {
Direction16 d;
Direction16 r;
d = Direction16::Degrees(45, 135);
r = -d;
EXPECT_EQ(r, Direction16::Degrees(-135, -135)) << "-(45, 135)";
d = Direction16::Degrees(-45, -135);
r = -d;
EXPECT_EQ(r, Direction16::Degrees(135, 135)) << "-(-45, -135)";
d = Direction16::Degrees(0, 0);
r = -d;
EXPECT_EQ(r, Direction16::Degrees(180, 0)) << "-(0, 0)";
d = Direction16::Degrees(0, 45);
r = -d;
EXPECT_EQ(r, Direction16::Degrees(180, -45)) << "-(0, 45)";
}
TEST(Direction16, Equality) {
Direction16 d;
d = Direction16::Degrees(135, 45);
EXPECT_EQ(d, Direction16::Degrees(135, 45)) << "(135, 45) == (135, 45)";
EXPECT_EQ(d, Direction16::Degrees(135 + 360, 45))
<< "(135, 45) == (135 + 360, 45) ";
EXPECT_EQ(d, Direction16::Degrees(135 - 360, 45))
<< "(135, 135) == (135 - 360, 45) ";
d = Direction16::Degrees(0, 45 + 180);
EXPECT_EQ(d, Direction16::Degrees(180, -45)) << "(0, 45+180) == (180, -45)";
}
#endif