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

Minor improvements

Browse Source
This commit is contained in:
Pascal Serrarens 2024-09-26 09:33:14 +02:00
parent 95a6fb3a4b
commit 8286c1ca85
4 changed files with 40 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
Angle.cpp
View File

@ -76,9 +76,8 @@ AngleOf<signed short> AngleOf<signed short>::Degrees(float angle) {
template <>
AngleOf<signed short> AngleOf<signed short>::Radians(float angle) {
if (!isfinite(angle)) {
if (!isfinite(angle))
return AngleOf<signed short>(0);
}
// map float [-PI..PI) to integer [-32768..32767]
signed short value = (signed short)(angle / pi * 32768.0F);
@ -130,6 +129,13 @@ float AngleOf<signed char>::InRadians() const {
//===== Generic
template <typename T>
const AngleOf<T> AngleOf<T>::zero = AngleOf<T>();
template <typename T>
const AngleOf<T> AngleOf<T>::deg90 = AngleOf<T>::Degrees(90);
template <typename T>
const AngleOf<T> AngleOf<T>::deg180 = AngleOf<T>::Degrees(180);
template <typename T>
bool AngleOf<T>::operator==(const AngleOf<T> a) const {
return this->value == a.value;
@ -267,6 +273,11 @@ AngleOf<T> AngleOf<T>::Atan(float f) {
return AngleOf<T>::Radians(atanf(f));
}
template <typename T>
AngleOf<T> Passer::LinearAlgebra::AngleOf<T>::Atan2(float f1, float f2) {
return AngleOf<T>::Radians(atan2f(f1, f2));
}
// template <>
// float AngleOf<float>::CosineRuleSide(float a, float b, AngleOf<float> gamma)
// {

5
Angle.h
View File

@ -21,6 +21,10 @@ class AngleOf {
static AngleOf<T> Degrees(float f);
static AngleOf<T> Radians(float f);
const static AngleOf<T> zero;
const static AngleOf<T> deg90;
const static AngleOf<T> deg180;
float InDegrees() const;
float InRadians() const;
@ -64,6 +68,7 @@ class AngleOf {
static AngleOf<T> Acos(float f);
static AngleOf<T> Asin(float f);
static AngleOf<T> Atan(float f);
static AngleOf<T> Atan2(float f1, float f2);
static float CosineRuleSide(float a, float b, AngleOf<T> gamma);
static AngleOf<T> CosineRuleAngle(float a, float b, float c);

33
Direction.cpp
View File

@ -24,8 +24,13 @@ DirectionOf<T>::DirectionOf(AngleOf<T> horizontal, AngleOf<T> vertical) {
template <typename T>
DirectionOf<T>::DirectionOf(Vector3 v) {
this->horizontal = AngleOf<T>::Radians(atan2f(v.Right(), v.Forward()));
this->vertical = AngleOf<T>::Radians(-(0.5f * pi) - acosf(v.Up()));
this->horizontal = AngleOf<T>::Atan2(
v.Right(),
v.Forward()); // AngleOf<T>::Radians(atan2f(v.Right(), v.Forward()));
this->vertical =
-AngleOf<T>::deg90 -
AngleOf<T>::Acos(
v.Up()); // AngleOf<T>::Radians(-(0.5f * pi) - acosf(v.Up()));
Normalize();
}
@ -34,19 +39,19 @@ const DirectionOf<T> DirectionOf<T>::forward =
DirectionOf<T>(AngleOf<T>(), AngleOf<T>());
template <typename T>
const DirectionOf<T> DirectionOf<T>::back =
DirectionOf<T>(AngleOf<T>::Degrees(180), AngleOf<T>());
DirectionOf<T>(AngleOf<T>::deg180, AngleOf<T>());
template <typename T>
const DirectionOf<T> DirectionOf<T>::up =
DirectionOf<T>(AngleOf<T>(), AngleOf<T>::Degrees(90));
DirectionOf<T>(AngleOf<T>(), AngleOf<T>::deg90);
template <typename T>
const DirectionOf<T> DirectionOf<T>::down =
DirectionOf<T>(AngleOf<T>(), AngleOf<T>::Degrees(-90));
DirectionOf<T>(AngleOf<T>(), -AngleOf<T>::deg90);
template <typename T>
const DirectionOf<T> DirectionOf<T>::left =
DirectionOf<T>(AngleOf<T>::Degrees(-90), AngleOf<T>());
DirectionOf<T>(-AngleOf<T>::deg90, AngleOf<T>());
template <typename T>
const DirectionOf<T> DirectionOf<T>::right =
DirectionOf<T>(AngleOf<T>::Degrees(90), AngleOf<T>());
DirectionOf<T>(AngleOf<T>::deg90, AngleOf<T>());
template <typename T>
DirectionOf<T> Passer::LinearAlgebra::DirectionOf<T>::Degrees(float horizontal,
@ -70,14 +75,14 @@ bool Passer::LinearAlgebra::DirectionOf<T>::operator==(
template <typename T>
DirectionOf<T> Passer::LinearAlgebra::DirectionOf<T>::operator-() const {
DirectionOf<T> r = DirectionOf<T>(this->horizontal + AngleOf<T>::Degrees(180),
-this->vertical);
DirectionOf<T> r =
DirectionOf<T>(this->horizontal + AngleOf<T>::deg180, -this->vertical);
return r;
}
template <typename T>
Vector3 DirectionOf<T>::ToVector3() {
Vector3 v = Quaternion::Euler(-(this->vertical.InDegrees()),
Vector3 v = Quaternion::Euler(-this->vertical.InDegrees(),
this->horizontal.InDegrees(), 0) *
Vector3::forward;
return v;
@ -85,10 +90,10 @@ Vector3 DirectionOf<T>::ToVector3() {
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;
if (this->vertical > AngleOf<T>::deg90 ||
this->vertical < -AngleOf<T>::deg90) {
this->horizontal += AngleOf<T>::deg180;
this->vertical = AngleOf<T>::deg180 - this->vertical;
}
}

8
SwingTwist.cpp
View File

@ -31,8 +31,7 @@ template <typename T>
SwingTwistOf<T> SwingTwistOf<T>::Degrees(float horizontal,
float vertical,
float twist) {
DirectionOf<T> swing = DirectionOf<T>(AngleOf<T>::Degrees(horizontal),
AngleOf<T>::Degrees(vertical));
DirectionOf<T> swing = DirectionOf<T>::Degrees(horizontal, vertical);
AngleOf<T> twistAngle = AngleOf<T>::Degrees(twist);
SwingTwistOf<T> orientation = SwingTwistOf(swing, twistAngle);
return orientation;
@ -50,9 +49,8 @@ template <typename T>
SwingTwistOf<T> Passer::LinearAlgebra::SwingTwistOf<T>::FromQuaternion(
Quaternion q) {
Vector3 angles = Quaternion::ToAngles(q);
SwingTwistOf<T> r = SwingTwistOf<T>(AngleOf<T>::Degrees(angles.Up()),
AngleOf<T>::Degrees(angles.Right()),
AngleOf<T>::Degrees(angles.Forward()));
SwingTwistOf<T> r =
SwingTwistOf<T>::Degrees(angles.Up(), angles.Right(), angles.Forward());
return r;
}