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

Improved Angle quality

Browse Source
This commit is contained in:
Pascal Serrarens 2024-09-25 09:39:44 +02:00
parent 9eca318991
commit 95a6fb3a4b
12 changed files with 872 additions and 126 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

192
Angle.cpp
View File

@ -19,57 +19,8 @@ float Angle::Normalize(float angle) {
angle -= 360;
return angle;
}
float Angle::Clamp(float angle, float min, float max) {
float normalizedAngle = Normalize(angle);
float r = Float::Clamp(normalizedAngle, min, max);
return r;
}
float Angle::Difference(float a, float b) {
float r = Normalize(b - a);
return r;
}
float Angle::MoveTowards(float fromAngle, float toAngle, float maxAngle) {
float d = toAngle - fromAngle;
float sign = signbit(d) ? -1 : 1;
d = sign * Float::Clamp(fabs(d), 0, maxAngle);
return fromAngle + d;
}
float Angle::CosineRuleSide(float a, float b, float gamma) {
float a2 = a * a;
float b2 = b * b;
float d = a2 + b2 - 2 * a * b * cos(gamma * Angle::Deg2Rad);
// Catch edge cases where float inacuracies lead tot nans
if (d < 0)
return 0;
float c = sqrtf(d);
return c;
}
float Angle::CosineRuleAngle(float a, float b, float c) {
float a2 = a * a;
float b2 = b * b;
float c2 = c * c;
float d = (a2 + b2 - c2) / (2 * a * b);
// Catch edge cases where float inacuracies lead tot nans
if (d >= 1)
return 0;
if (d <= -1)
return 180;
float gamma = acos(d) * Angle::Rad2Deg;
return gamma;
}
float Angle::SineRuleAngle(float a, float beta, float b) {
float alpha = asin(a * sin(beta * Angle::Deg2Rad) / b);
return alpha;
}
*/
//----------------------
template <typename T>
@ -82,11 +33,25 @@ AngleOf<T>::AngleOf(T angle) : value(angle) {}
template <>
AngleOf<float> AngleOf<float>::Degrees(float angle) {
if (isfinite(angle)) {
while (angle < -180)
angle += 360;
while (angle >= 180)
angle -= 360;
}
return AngleOf(angle);
}
template <>
AngleOf<float> AngleOf<float>::Radians(float angle) {
if (isfinite(angle)) {
while (angle <= -pi)
angle += 2 * pi;
while (angle > pi)
angle -= 2 * pi;
}
return AngleOf(angle * Rad2Deg);
}
@ -104,10 +69,6 @@ float AngleOf<float>::InRadians() const {
template <>
AngleOf<signed short> AngleOf<signed short>::Degrees(float angle) {
if (!isfinite(angle)) {
return AngleOf<signed short>(0);
}
// map float [-180..180) to integer [-32768..32767]
signed short value = (signed short)(angle / 360.0F * 65536.0F);
return AngleOf<signed short>(value);
@ -140,9 +101,6 @@ float AngleOf<signed short>::InRadians() const {
template <>
AngleOf<signed char> AngleOf<signed char>::Degrees(float angle) {
if (!isfinite(angle))
return AngleOf<signed char>(0);
// map float [-180..180) to integer [-128..127)
signed char value = (signed char)(angle / 360.0F * 256.0F);
return AngleOf<signed char>(value);
@ -197,6 +155,23 @@ bool AngleOf<T>::operator<=(AngleOf<T> a) {
return this->value <= a.value;
}
template <typename T>
signed int Passer::LinearAlgebra::AngleOf<T>::Sign(AngleOf<T> a) {
if (a.value < 0)
return -1;
if (a.value > 0)
return 1;
return 0;
}
template <typename T>
AngleOf<T> Passer::LinearAlgebra::AngleOf<T>::Abs(AngleOf<T> a) {
if (Sign(a) < 0)
return -a;
else
return a;
}
template <typename T>
AngleOf<T> AngleOf<T>::operator-() const {
AngleOf<T> angle = AngleOf(-this->value);
@ -246,23 +221,24 @@ AngleOf<T> AngleOf<T>::Normalize(AngleOf<T> angle) {
return AngleOf::Degrees(angleValue);
}
template <>
AngleOf<float> AngleOf<float>::Clamp(AngleOf<float> angle,
AngleOf<float> min,
AngleOf<float> max) {
float normalizedAngle = Normalize(angle).InDegrees();
float r = Float::Clamp(normalizedAngle, min.InDegrees(), max.InDegrees());
return r;
template <typename T>
AngleOf<T> AngleOf<T>::Clamp(AngleOf<T> angle, AngleOf<T> min, AngleOf<T> max) {
float r = Float::Clamp(angle.InDegrees(), min.InDegrees(), max.InDegrees());
return AngleOf<T>::Degrees(r);
}
template <>
AngleOf<float> AngleOf<float>::MoveTowards(AngleOf<float> fromAngle,
AngleOf<float> toAngle,
AngleOf<float> maxAngle) {
float d = toAngle.InDegrees() - fromAngle.InDegrees();
int sign = signbit(d) ? -1 : 1;
d = sign * Float::Clamp(fabsf(d), 0, maxAngle.InDegrees());
return fromAngle.InDegrees() + d;
template <typename T>
AngleOf<T> AngleOf<T>::MoveTowards(AngleOf<T> fromAngle,
AngleOf<T> toAngle,
float maxDegrees) {
maxDegrees = fmaxf(0, maxDegrees); // filter out negative distances
AngleOf<T> d = toAngle - fromAngle;
float dDegrees = Abs(d).InDegrees();
d = AngleOf<T>::Degrees(Float::Clamp(dDegrees, 0, maxDegrees));
if (Sign(d) < 0)
d = -d;
return fromAngle + d;
}
template <typename T>
@ -291,41 +267,83 @@ AngleOf<T> AngleOf<T>::Atan(float f) {
return AngleOf<T>::Radians(atanf(f));
}
template <>
AngleOf<float> AngleOf<float>::CosineRuleSide(float a, float b, float gamma) {
// template <>
// float AngleOf<float>::CosineRuleSide(float a, float b, AngleOf<float> gamma)
// {
// float a2 = a * a;
// float b2 = b * b;
// float d =
// a2 + b2 -
// 2 * a * b * Cos(gamma); // cosf(gamma *
// Passer::LinearAlgebra::Deg2Rad);
// // Catch edge cases where float inacuracies lead tot nans
// if (d < 0)
// return 0.0f;
// float c = sqrtf(d);
// return c;
// }
template <typename T>
float AngleOf<T>::CosineRuleSide(float a, float b, AngleOf<T> gamma) {
float a2 = a * a;
float b2 = b * b;
float d = a2 + b2 - 2 * a * b * cosf(gamma * Passer::LinearAlgebra::Deg2Rad);
float d =
a2 + b2 -
2 * a * b * Cos(gamma); // cosf(gamma * Passer::LinearAlgebra::Deg2Rad);
// Catch edge cases where float inacuracies lead tot nans
if (d < 0)
return 0.0f;
return 0;
float c = sqrtf(d);
return c;
}
template <>
AngleOf<float> AngleOf<float>::CosineRuleAngle(float a, float b, float c) {
// template <>
// AngleOf<float> AngleOf<float>::CosineRuleAngle(float a, float b, float c) {
// float a2 = a * a;
// float b2 = b * b;
// float c2 = c * c;
// float d = (a2 + b2 - c2) / (2 * a * b);
// // Catch edge cases where float inacuracies lead tot nans
// if (d >= 1)
// return 0.0f;
// if (d <= -1)
// return 180.0f;
// float gamma = acosf(d) * Rad2Deg;
// return gamma;
// }
template <typename T>
AngleOf<T> AngleOf<T>::CosineRuleAngle(float a, float b, float c) {
float a2 = a * a;
float b2 = b * b;
float c2 = c * c;
float d = (a2 + b2 - c2) / (2 * a * b);
// Catch edge cases where float inacuracies lead tot nans
if (d >= 1)
return 0.0f;
return AngleOf<T>();
if (d <= -1)
return 180.0f;
return AngleOf<T>::Degrees(180);
float gamma = acosf(d) * Rad2Deg;
// float gamma = acosf(d) * Rad2Deg;
AngleOf<T> gamma = Acos(d);
return gamma;
}
template <>
AngleOf<float> AngleOf<float>::SineRuleAngle(float a,
AngleOf<float> beta,
float b) {
float deg2rad = Deg2Rad;
float alpha = asinf(a * sinf(beta.InDegrees() * deg2rad) / b);
// template <>
// AngleOf<float> AngleOf<float>::SineRuleAngle(float a,
// AngleOf<float> beta,
// float b) {
// float deg2rad = Deg2Rad;
// float alpha = asinf(a * sinf(beta.InDegrees() * deg2rad) / b);
// return alpha;
// }
template <typename T>
AngleOf<T> AngleOf<T>::SineRuleAngle(float a, AngleOf<T> beta, float b) {
// float deg2rad = Deg2Rad;
// float alpha = asinf(a * sinf(beta.InDegrees() * deg2rad) / b);
AngleOf<T> alpha = Asin(a * Sin(beta) / b);
return alpha;
}

10
Angle.h
View File

@ -21,20 +21,20 @@ class AngleOf {
static AngleOf<T> Degrees(float f);
static AngleOf<T> Radians(float f);
// float ToFloat() const;
float InDegrees() const;
float InRadians() const;
inline T GetBinary() const { return this->value; }
// static AngleOf<T> pi;
bool operator==(const AngleOf<T> a) const;
bool operator>(AngleOf<T> a);
bool operator>=(AngleOf<T> a);
bool operator<(AngleOf<T> a);
bool operator<=(AngleOf<T> a);
static signed int Sign(AngleOf<T> a);
static AngleOf<T> Abs(AngleOf<T> a);
AngleOf<T> operator-() const;
AngleOf<T> operator-(const AngleOf<T>& a) const;
AngleOf<T> operator+(const AngleOf<T>& a) const;
@ -55,7 +55,7 @@ class AngleOf {
// };
static AngleOf<T> MoveTowards(AngleOf<T> fromAngle,
AngleOf<T> toAngle,
AngleOf<T> maxAngle);
float maxAngle);
static float Cos(AngleOf<T> a);
static float Sin(AngleOf<T> a);
@ -65,7 +65,7 @@ class AngleOf {
static AngleOf<T> Asin(float f);
static AngleOf<T> Atan(float f);
static AngleOf<T> CosineRuleSide(float a, float b, float gamma);
static float CosineRuleSide(float a, float b, AngleOf<T> gamma);
static AngleOf<T> CosineRuleAngle(float a, float b, float c);
static AngleOf<T> SineRuleAngle(float a, AngleOf<T> beta, float c);

2
FloatSingle.cpp
View File

@ -2,8 +2,8 @@
// License, v. 2.0.If a copy of the MPL was not distributed with this
// file, You can obtain one at https ://mozilla.org/MPL/2.0/.
#include <math.h>
#include "FloatSingle.h"
#include <math.h>
const float Float::epsilon = 1e-05f;
const float Float::sqrEpsilon = 1e-10f;

9
Polar.cpp
View File

@ -102,8 +102,8 @@ PolarOf<T> PolarOf<T>::operator+(const PolarOf& v) const {
return PolarOf(this->distance + v.distance, this->angle);
}
float newDistance =
Angle::CosineRuleSide(v.distance, this->distance, rotation).InDegrees();
float newDistance = AngleOf<T>::CosineRuleSide(v.distance, this->distance,
AngleOf<T>::Degrees(rotation));
float angle = Angle::CosineRuleAngle(newDistance, this->distance, v.distance)
.InDegrees();
@ -133,9 +133,8 @@ PolarOf<T> PolarOf<T>::operator/=(float f) {
template <typename T>
float PolarOf<T>::Distance(const PolarOf& v1, const PolarOf& v2) {
float d = Angle::CosineRuleSide(v1.distance, v2.distance,
v2.angle.InDegrees() - v1.angle.InDegrees())
.InDegrees();
float d =
AngleOf<T>::CosineRuleSide(v1.distance, v2.distance, v2.angle - v1.angle);
return d;
}

239
test/Angle16_test.cc Normal file
View File

@ -0,0 +1,239 @@
#if GTEST
#include <gtest/gtest.h>
#include <math.h>
#include <limits>
#include "Angle.h"
#define FLOAT_INFINITY std::numeric_limits<float>::infinity()
TEST(Angle16, Construct) {
float angle = 0.0F;
Angle16 a = Angle16::Degrees(angle);
EXPECT_FLOAT_EQ(a.InDegrees(), angle);
angle = -180.0F;
a = Angle16::Degrees(angle);
EXPECT_FLOAT_EQ(a.InDegrees(), angle);
angle = 270.0F;
a = Angle16::Degrees(angle);
EXPECT_FLOAT_EQ(a.InDegrees(), -90);
}
TEST(Angle16, Negate) {
float angle = 0;
Angle16 a = Angle16::Degrees(angle);
a = -a;
EXPECT_FLOAT_EQ(a.InDegrees(), angle);
angle = 90.0F;
a = Angle16::Degrees(angle);
a = -a;
EXPECT_FLOAT_EQ(a.InDegrees(), -angle);
}
TEST(Angle16, Subtract) {
Angle16 a = Angle16::Degrees(0);
Angle16 b = Angle16::Degrees(45.0F);
Angle16 r = a - b;
EXPECT_FLOAT_EQ(r.InDegrees(), -45);
}
TEST(Angle16, Add) {
Angle16 a = Angle16::Degrees(-45);
Angle16 b = Angle16::Degrees(45.0F);
Angle16 r = a + b;
EXPECT_FLOAT_EQ(r.InDegrees(), 0);
}
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(Angle16, Normalize) {
Angle16 r = Angle16();
r = Angle16::Normalize(Angle16::Degrees(90.0f));
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Normalize 90";
r = Angle16::Normalize(Angle16::Degrees(-90));
EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Normalize -90";
r = Angle16::Normalize(Angle16::Degrees(270));
EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Normalize 270";
r = Angle16::Normalize(Angle16::Degrees(270 + 360));
EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Normalize 270+360";
r = Angle16::Normalize(Angle16::Degrees(-270));
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Normalize -270";
r = Angle16::Normalize(Angle16::Degrees(-270 - 360));
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Normalize -270-360";
r = Angle16::Normalize(Angle16::Degrees(0));
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Normalize 0";
if (false) { // std::numeric_limits<float>::is_iec559) {
// Infinites are not supported
r = Angle16::Normalize(Angle16::Degrees(FLOAT_INFINITY));
EXPECT_FLOAT_EQ(r.InDegrees(), FLOAT_INFINITY) << "Normalize INFINITY";
r = Angle16::Normalize(Angle16::Degrees(-FLOAT_INFINITY));
EXPECT_FLOAT_EQ(r.InDegrees(), -FLOAT_INFINITY) << "Normalize INFINITY";
}
}
TEST(Angle16, Clamp) {
Angle16 r = Angle16();
// Clamp(1, 0, 2) will fail because Angle16 does not have enough resolution
// for this. Instead we use Clamp(10, 0, 20) etc.
r = Angle16::Clamp(Angle16::Degrees(10), Angle16::Degrees(0),
Angle16::Degrees(20));
EXPECT_NEAR(r.InDegrees(), 10, 1.0e-2) << "Clamp 10 0 20";
r = Angle16::Clamp(Angle16::Degrees(-10), Angle16::Degrees(0),
Angle16::Degrees(20));
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Clamp -10 0 20";
r = Angle16::Clamp(Angle16::Degrees(30), Angle16::Degrees(0),
Angle16::Degrees(20));
EXPECT_NEAR(r.InDegrees(), 20, 1.0e-2) << "Clamp 30 0 20";
r = Angle16::Clamp(Angle16::Degrees(10), Angle16::Degrees(0),
Angle16::Degrees(0));
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Clamp 10 0 0";
r = Angle16::Clamp(Angle16::Degrees(0), Angle16::Degrees(0),
Angle16::Degrees(0));
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Clamp 0 0 0";
r = Angle16::Clamp(Angle16::Degrees(0), Angle16::Degrees(10),
Angle16::Degrees(-10));
EXPECT_NEAR(r.InDegrees(), 10, 1.0e-2) << "Clamp 0 10 -10";
if (false) { // std::numeric_limits<float>::is_iec559) {
// Infinites are not supported
r = Angle16::Clamp(Angle16::Degrees(10), Angle16::Degrees(0),
Angle16::Degrees(FLOAT_INFINITY));
EXPECT_NEAR(r.InDegrees(), 10, 1.0e-2) << "Clamp 1 0 INFINITY";
r = Angle16::Clamp(Angle16::Degrees(10), Angle16::Degrees(-FLOAT_INFINITY),
Angle16::Degrees(10));
EXPECT_NEAR(r.InDegrees(), 10, 1.0e-2) << "Clamp 1 -INFINITY 1";
}
}
// TEST(Angle16, Difference) {
// Angle16 r = 0;
// r = Angle16::Difference(0, 90);
// EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Difference 0 90";
// r = Angle16::Difference(0, -90);
// EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Difference 0 -90";
// r = Angle16::Difference(0, 270);
// EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Difference 0 270";
// r = Angle16::Difference(0, -270);
// EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Difference 0 -270";
// r = Angle16::Difference(90, 0);
// EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Difference 90 0";
// r = Angle16::Difference(-90, 0);
// EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Difference -90 0";
// r = Angle16::Difference(0, 0);
// EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Difference 0 0";
// r = Angle16::Difference(90, 90);
// EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Difference 90 90";
// if (std::numeric_limits<float>::is_iec559) {
// r = Angle16::Difference(0, INFINITY);
// EXPECT_FLOAT_EQ(r.InDegrees(), INFINITY) << "Difference 0 INFINITY";
// r = Angle16::Difference(0, -INFINITY);
// EXPECT_FLOAT_EQ(r.InDegrees(), -INFINITY) << "Difference 0 -INFINITY";
// r = Angle16::Difference(-INFINITY, INFINITY);
// EXPECT_FLOAT_EQ(r.InDegrees(), INFINITY) << "Difference -INFINITY
// INFINITY";
// }
// }
TEST(Angle16, MoveTowards) {
Angle16 r = Angle16();
r = Angle16::MoveTowards(Angle16::Degrees(0), Angle16::Degrees(90), 30);
EXPECT_NEAR(r.InDegrees(), 30, 1.0e-2) << "MoveTowards 0 90 30";
r = Angle16::MoveTowards(Angle16::Degrees(0), Angle16::Degrees(90), 90);
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "MoveTowards 0 90 90";
r = Angle16::MoveTowards(Angle16::Degrees(0), Angle16::Degrees(-90), 180);
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "MoveTowards 0 -90 -180";
r = Angle16::MoveTowards(Angle16::Degrees(0), Angle16::Degrees(90), 270);
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "MoveTowards 0 90 270";
r = Angle16::MoveTowards(Angle16::Degrees(0), Angle16::Degrees(90), -30);
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 90 -30";
r = Angle16::MoveTowards(Angle16::Degrees(0), Angle16::Degrees(-90), -30);
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 -90 -30";
r = Angle16::MoveTowards(Angle16::Degrees(0), Angle16::Degrees(-90), -90);
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 -90 -90";
r = Angle16::MoveTowards(Angle16::Degrees(0), Angle16::Degrees(-90), -180);
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 -90 -180";
r = Angle16::MoveTowards(Angle16::Degrees(0), Angle16::Degrees(-90), -270);
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 -90 -270";
r = Angle16::MoveTowards(Angle16::Degrees(0), Angle16::Degrees(90), 0);
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 90 0";
r = Angle16::MoveTowards(Angle16::Degrees(0), Angle16::Degrees(0), 0);
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 0 0";
r = Angle16::MoveTowards(Angle16::Degrees(0), Angle16::Degrees(0), 30);
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 0 30";
if (false) { // std::numeric_limits<float>::is_iec559) {
// infinites are not supported
r = Angle16::MoveTowards(Angle16::Degrees(0), Angle16::Degrees(90),
FLOAT_INFINITY);
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "MoveTowards 0 90 FLOAT_INFINITY";
r = Angle16::MoveTowards(Angle16::Degrees(0),
Angle16::Degrees(FLOAT_INFINITY), 30);
EXPECT_FLOAT_EQ(r.InDegrees(), 30) << "MoveTowards 0 FLOAT_INFINITY 30";
r = Angle16::MoveTowards(Angle16::Degrees(0), Angle16::Degrees(-90),
-FLOAT_INFINITY);
EXPECT_FLOAT_EQ(r.InDegrees(), FLOAT_INFINITY)
<< "MoveTowards 0 -90 -FLOAT_INFINITY";
r = Angle16::MoveTowards(Angle16::Degrees(0),
Angle16::Degrees(-FLOAT_INFINITY), -30);
EXPECT_FLOAT_EQ(r.InDegrees(), 30) << "MoveTowards 0 -FLOAT_INFINITY -30";
}
}
#endif

239
test/Angle8_test.cc Normal file
View File

@ -0,0 +1,239 @@
#if GTEST
#include <gtest/gtest.h>
#include <math.h>
#include <limits>
#include "Angle.h"
#define FLOAT_INFINITY std::numeric_limits<float>::infinity()
TEST(Angle8, Construct) {
float angle = 0.0F;
Angle8 a = Angle8::Degrees(angle);
EXPECT_FLOAT_EQ(a.InDegrees(), angle);
angle = -180.0F;
a = Angle8::Degrees(angle);
EXPECT_FLOAT_EQ(a.InDegrees(), angle);
angle = 270.0F;
a = Angle8::Degrees(angle);
EXPECT_FLOAT_EQ(a.InDegrees(), -90);
}
TEST(Angle8, Negate) {
float angle = 0;
Angle8 a = Angle8::Degrees(angle);
a = -a;
EXPECT_FLOAT_EQ(a.InDegrees(), angle);
angle = 90.0F;
a = Angle8::Degrees(angle);
a = -a;
EXPECT_FLOAT_EQ(a.InDegrees(), -angle);
}
TEST(Angle8, Add) {
Angle8 a = Angle8::Degrees(-45);
Angle8 b = Angle8::Degrees(45.0F);
Angle8 r = a + b;
EXPECT_FLOAT_EQ(r.InDegrees(), 0);
}
TEST(Angle8, Subtract) {
Angle8 a = Angle8::Degrees(0);
Angle8 b = Angle8::Degrees(45.0F);
Angle8 r = a - b;
EXPECT_FLOAT_EQ(r.InDegrees(), -45);
}
TEST(Angle8, Compare) {
Angle8 a = Angle8::Degrees(45);
bool r = false;
r = a > Angle8::Degrees(0);
EXPECT_TRUE(r) << "45 > 0";
r = a > Angle8::Degrees(90);
EXPECT_FALSE(r) << "45 > 90";
r = a > Angle8::Degrees(-90);
EXPECT_TRUE(r) << "45 > -90";
}
TEST(Angle8, Normalize) {
Angle8 r = Angle8();
r = Angle8::Normalize(Angle8::Degrees(90.0f));
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Normalize 90";
r = Angle8::Normalize(Angle8::Degrees(-90));
EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Normalize -90";
r = Angle8::Normalize(Angle8::Degrees(270));
EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Normalize 270";
r = Angle8::Normalize(Angle8::Degrees(270 + 360));
EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Normalize 270+360";
r = Angle8::Normalize(Angle8::Degrees(-270));
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Normalize -270";
r = Angle8::Normalize(Angle8::Degrees(-270 - 360));
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Normalize -270-360";
r = Angle8::Normalize(Angle8::Degrees(0));
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Normalize 0";
if (false) { // std::numeric_limits<float>::is_iec559) {
// Infinites are not supported
r = Angle8::Normalize(Angle8::Degrees(FLOAT_INFINITY));
EXPECT_FLOAT_EQ(r.InDegrees(), FLOAT_INFINITY) << "Normalize INFINITY";
r = Angle8::Normalize(Angle8::Degrees(-FLOAT_INFINITY));
EXPECT_FLOAT_EQ(r.InDegrees(), -FLOAT_INFINITY) << "Normalize INFINITY";
}
}
TEST(Angle8, Clamp) {
Angle8 r = Angle8();
// Clamp(1, 0, 2) will fail because Angle8 does not have enough resolution for
// this. Instead we use Clamp(10, 0, 20) etc.
r = Angle8::Clamp(Angle8::Degrees(10), Angle8::Degrees(0),
Angle8::Degrees(20));
EXPECT_NEAR(r.InDegrees(), 10, 1.0e-0) << "Clamp 10 0 20";
r = Angle8::Clamp(Angle8::Degrees(-10), Angle8::Degrees(0),
Angle8::Degrees(20));
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Clamp -10 0 20";
r = Angle8::Clamp(Angle8::Degrees(30), Angle8::Degrees(0),
Angle8::Degrees(20));
EXPECT_NEAR(r.InDegrees(), 20, 1.0e-0) << "Clamp 30 0 20";
r = Angle8::Clamp(Angle8::Degrees(10), Angle8::Degrees(0),
Angle8::Degrees(0));
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Clamp 10 0 0";
r = Angle8::Clamp(Angle8::Degrees(0), Angle8::Degrees(0), Angle8::Degrees(0));
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Clamp 0 0 0";
r = Angle8::Clamp(Angle8::Degrees(0), Angle8::Degrees(10),
Angle8::Degrees(-10));
EXPECT_NEAR(r.InDegrees(), 10, 1.0e-0) << "Clamp 0 10 -10";
if (false) { // std::numeric_limits<float>::is_iec559) {
// Infinites are not supported
r = Angle8::Clamp(Angle8::Degrees(10), Angle8::Degrees(0),
Angle8::Degrees(FLOAT_INFINITY));
EXPECT_NEAR(r.InDegrees(), 10, 1.0e-0) << "Clamp 1 0 INFINITY";
r = Angle8::Clamp(Angle8::Degrees(10), Angle8::Degrees(-FLOAT_INFINITY),
Angle8::Degrees(10));
EXPECT_NEAR(r.InDegrees(), 10, 1.0e-0) << "Clamp 1 -INFINITY 1";
}
}
// TEST(Angle8, Difference) {
// Angle8 r = 0;
// r = Angle8::Difference(0, 90);
// EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Difference 0 90";
// r = Angle8::Difference(0, -90);
// EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Difference 0 -90";
// r = Angle8::Difference(0, 270);
// EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Difference 0 270";
// r = Angle8::Difference(0, -270);
// EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Difference 0 -270";
// r = Angle8::Difference(90, 0);
// EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Difference 90 0";
// r = Angle8::Difference(-90, 0);
// EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Difference -90 0";
// r = Angle8::Difference(0, 0);
// EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Difference 0 0";
// r = Angle8::Difference(90, 90);
// EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Difference 90 90";
// if (std::numeric_limits<float>::is_iec559) {
// r = Angle8::Difference(0, INFINITY);
// EXPECT_FLOAT_EQ(r.InDegrees(), INFINITY) << "Difference 0 INFINITY";
// r = Angle8::Difference(0, -INFINITY);
// EXPECT_FLOAT_EQ(r.InDegrees(), -INFINITY) << "Difference 0 -INFINITY";
// r = Angle8::Difference(-INFINITY, INFINITY);
// EXPECT_FLOAT_EQ(r.InDegrees(), INFINITY) << "Difference -INFINITY
// INFINITY";
// }
// }
TEST(Angle8, MoveTowards) {
Angle8 r = Angle8();
r = Angle8::MoveTowards(Angle8::Degrees(0), Angle8::Degrees(90), 30);
EXPECT_NEAR(r.InDegrees(), 30, 1.0e-0) << "MoveTowards 0 90 30";
r = Angle8::MoveTowards(Angle8::Degrees(0), Angle8::Degrees(90), 90);
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "MoveTowards 0 90 90";
r = Angle8::MoveTowards(Angle8::Degrees(0), Angle8::Degrees(-90), 180);
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "MoveTowards 0 -90 -180";
r = Angle8::MoveTowards(Angle8::Degrees(0), Angle8::Degrees(90), 270);
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "MoveTowards 0 90 270";
r = Angle8::MoveTowards(Angle8::Degrees(0), Angle8::Degrees(90), -30);
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 90 -30";
r = Angle8::MoveTowards(Angle8::Degrees(0), Angle8::Degrees(-90), -30);
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 -90 -30";
r = Angle8::MoveTowards(Angle8::Degrees(0), Angle8::Degrees(-90), -90);
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 -90 -90";
r = Angle8::MoveTowards(Angle8::Degrees(0), Angle8::Degrees(-90), -180);
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 -90 -180";
r = Angle8::MoveTowards(Angle8::Degrees(0), Angle8::Degrees(-90), -270);
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 -90 -270";
r = Angle8::MoveTowards(Angle8::Degrees(0), Angle8::Degrees(90), 0);
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 90 0";
r = Angle8::MoveTowards(Angle8::Degrees(0), Angle8::Degrees(0), 0);
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 0 0";
r = Angle8::MoveTowards(Angle8::Degrees(0), Angle8::Degrees(0), 30);
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 0 30";
if (false) { // std::numeric_limits<float>::is_iec559) {
// infinites are not supported
r = Angle8::MoveTowards(Angle8::Degrees(0), Angle8::Degrees(90),
FLOAT_INFINITY);
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "MoveTowards 0 90 FLOAT_INFINITY";
r = Angle8::MoveTowards(Angle8::Degrees(0), Angle8::Degrees(FLOAT_INFINITY),
30);
EXPECT_FLOAT_EQ(r.InDegrees(), 30) << "MoveTowards 0 FLOAT_INFINITY 30";
r = Angle8::MoveTowards(Angle8::Degrees(0), Angle8::Degrees(-90),
-FLOAT_INFINITY);
EXPECT_FLOAT_EQ(r.InDegrees(), FLOAT_INFINITY)
<< "MoveTowards 0 -90 -FLOAT_INFINITY";
r = Angle8::MoveTowards(Angle8::Degrees(0),
Angle8::Degrees(-FLOAT_INFINITY), -30);
EXPECT_FLOAT_EQ(r.InDegrees(), 30) << "MoveTowards 0 -FLOAT_INFINITY -30";
}
}
#endif

247
test/AngleSingle_test.cc Normal file
View File

@ -0,0 +1,247 @@
#if GTEST
#include <gtest/gtest.h>
#include <math.h>
#include <limits>
#include "Angle.h"
#define FLOAT_INFINITY std::numeric_limits<float>::infinity()
TEST(AngleSingle, Construct) {
float angle = 0.0F;
AngleSingle a = AngleSingle::Degrees(angle);
EXPECT_FLOAT_EQ(a.InDegrees(), angle);
angle = -180.0F;
a = AngleSingle::Degrees(angle);
EXPECT_FLOAT_EQ(a.InDegrees(), angle);
angle = 270.0F;
a = AngleSingle::Degrees(angle);
EXPECT_FLOAT_EQ(a.InDegrees(), -90);
}
TEST(AngleSingle, Negate) {
float angle = 0;
AngleSingle a = AngleSingle::Degrees(angle);
a = -a;
EXPECT_FLOAT_EQ(a.InDegrees(), angle);
angle = 90.0F;
a = AngleSingle::Degrees(angle);
a = -a;
EXPECT_FLOAT_EQ(a.InDegrees(), -angle);
}
TEST(AngleSingle, Add) {
AngleSingle a = AngleSingle::Degrees(-45);
AngleSingle b = AngleSingle::Degrees(45.0F);
AngleSingle r = a + b;
EXPECT_FLOAT_EQ(r.InDegrees(), 0);
}
TEST(AngleSingle, Subtract) {
AngleSingle a = AngleSingle::Degrees(0);
AngleSingle b = AngleSingle::Degrees(45.0F);
AngleSingle r = a - b;
EXPECT_FLOAT_EQ(r.InDegrees(), -45);
}
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(AngleSingle, Normalize) {
Angle r = AngleSingle();
r = AngleSingle::Normalize(AngleSingle::Degrees(90.0f));
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Normalize 90";
r = AngleSingle::Normalize(AngleSingle::Degrees(-90));
EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Normalize -90";
r = AngleSingle::Normalize(AngleSingle::Degrees(270));
EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Normalize 270";
r = AngleSingle::Normalize(AngleSingle::Degrees(270 + 360));
EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Normalize 270+360";
r = AngleSingle::Normalize(AngleSingle::Degrees(-270));
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Normalize -270";
r = AngleSingle::Normalize(AngleSingle::Degrees(-270 - 360));
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Normalize -270-360";
r = AngleSingle::Normalize(AngleSingle::Degrees(0));
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Normalize 0";
if (std::numeric_limits<float>::is_iec559) {
r = AngleSingle::Normalize(AngleSingle::Degrees(FLOAT_INFINITY));
EXPECT_FLOAT_EQ(r.InDegrees(), FLOAT_INFINITY) << "Normalize INFINITY";
r = AngleSingle::Normalize(AngleSingle::Degrees(-FLOAT_INFINITY));
EXPECT_FLOAT_EQ(r.InDegrees(), -FLOAT_INFINITY) << "Normalize INFINITY";
}
}
TEST(AngleSingle, Clamp) {
Angle r = AngleSingle();
r = AngleSingle::Clamp(AngleSingle::Degrees(1), AngleSingle::Degrees(0),
AngleSingle::Degrees(2));
EXPECT_FLOAT_EQ(r.InDegrees(), 1) << "Clamp 1 0 2";
r = AngleSingle::Clamp(AngleSingle::Degrees(-1), AngleSingle::Degrees(0),
AngleSingle::Degrees(2));
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Clamp -1 0 2";
r = AngleSingle::Clamp(AngleSingle::Degrees(3), AngleSingle::Degrees(0),
AngleSingle::Degrees(2));
EXPECT_FLOAT_EQ(r.InDegrees(), 2) << "Clamp 3 0 2";
r = AngleSingle::Clamp(AngleSingle::Degrees(1), AngleSingle::Degrees(0),
AngleSingle::Degrees(0));
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Clamp 1 0 0";
r = AngleSingle::Clamp(AngleSingle::Degrees(0), AngleSingle::Degrees(0),
AngleSingle::Degrees(0));
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Clamp 0 0 0";
r = AngleSingle::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 = AngleSingle::Clamp(AngleSingle::Degrees(1), AngleSingle::Degrees(0),
AngleSingle::Degrees(FLOAT_INFINITY));
EXPECT_FLOAT_EQ(r.InDegrees(), 1) << "Clamp 1 0 INFINITY";
r = AngleSingle::Clamp(AngleSingle::Degrees(1),
AngleSingle::Degrees(-FLOAT_INFINITY),
AngleSingle::Degrees(1));
EXPECT_FLOAT_EQ(r.InDegrees(), 1) << "Clamp 1 -INFINITY 1";
}
}
// TEST(AngleSingle, Difference) {
// Angle r = 0;
// r = AngleSingle::Difference(0, 90);
// EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Difference 0 90";
// r = AngleSingle::Difference(0, -90);
// EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Difference 0 -90";
// r = AngleSingle::Difference(0, 270);
// EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Difference 0 270";
// r = AngleSingle::Difference(0, -270);
// EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Difference 0 -270";
// r = AngleSingle::Difference(90, 0);
// EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Difference 90 0";
// r = AngleSingle::Difference(-90, 0);
// EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Difference -90 0";
// r = AngleSingle::Difference(0, 0);
// EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Difference 0 0";
// r = AngleSingle::Difference(90, 90);
// EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Difference 90 90";
// if (std::numeric_limits<float>::is_iec559) {
// r = AngleSingle::Difference(0, INFINITY);
// EXPECT_FLOAT_EQ(r.InDegrees(), INFINITY) << "Difference 0 INFINITY";
// r = AngleSingle::Difference(0, -INFINITY);
// EXPECT_FLOAT_EQ(r.InDegrees(), -INFINITY) << "Difference 0 -INFINITY";
// r = AngleSingle::Difference(-INFINITY, INFINITY);
// EXPECT_FLOAT_EQ(r.InDegrees(), INFINITY) << "Difference -INFINITY
// INFINITY";
// }
// }
TEST(AngleSingle, MoveTowards) {
AngleSingle r = AngleSingle();
r = AngleSingle::MoveTowards(AngleSingle::Degrees(0),
AngleSingle::Degrees(90), 30);
EXPECT_FLOAT_EQ(r.InDegrees(), 30) << "MoveTowards 0 90 30";
r = AngleSingle::MoveTowards(AngleSingle::Degrees(0),
AngleSingle::Degrees(90), 90);
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "MoveTowards 0 90 90";
r = AngleSingle::MoveTowards(AngleSingle::Degrees(0),
AngleSingle::Degrees(90), 180);
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "MoveTowards 0 90 180";
r = AngleSingle::MoveTowards(AngleSingle::Degrees(0),
AngleSingle::Degrees(90), 270);
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "MoveTowards 0 90 270";
r = AngleSingle::MoveTowards(AngleSingle::Degrees(0),
AngleSingle::Degrees(90), -30);
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 90 -30";
r = AngleSingle::MoveTowards(AngleSingle::Degrees(0),
AngleSingle::Degrees(-90), -30);
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 -90 -30";
r = AngleSingle::MoveTowards(AngleSingle::Degrees(0),
AngleSingle::Degrees(-90), -90);
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 -90 -90";
r = AngleSingle::MoveTowards(AngleSingle::Degrees(0),
AngleSingle::Degrees(-90), -180);
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 -90 -180";
r = AngleSingle::MoveTowards(AngleSingle::Degrees(0),
AngleSingle::Degrees(-90), -270);
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 -90 -270";
r = AngleSingle::MoveTowards(AngleSingle::Degrees(0),
AngleSingle::Degrees(90), 0);
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 90 0";
r = AngleSingle::MoveTowards(AngleSingle::Degrees(0), AngleSingle::Degrees(0),
0);
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 0 0";
r = AngleSingle::MoveTowards(AngleSingle::Degrees(0), AngleSingle::Degrees(0),
30);
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 0 30";
if (std::numeric_limits<float>::is_iec559) {
r = AngleSingle::MoveTowards(AngleSingle::Degrees(0),
AngleSingle::Degrees(90), FLOAT_INFINITY);
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "MoveTowards 0 90 FLOAT_INFINITY";
r = AngleSingle::MoveTowards(AngleSingle::Degrees(0),
AngleSingle::Degrees(FLOAT_INFINITY), 30);
EXPECT_FLOAT_EQ(r.InDegrees(), 30) << "MoveTowards 0 FLOAT_INFINITY 30";
r = AngleSingle::MoveTowards(AngleSingle::Degrees(0),
AngleSingle::Degrees(-90), -FLOAT_INFINITY);
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 -90 -FLOAT_INFINITY";
r = AngleSingle::MoveTowards(AngleSingle::Degrees(0),
AngleSingle::Degrees(-FLOAT_INFINITY), -30);
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 -FLOAT_INFINITY -30";
}
}
#endif

2
test/Angle_test.cc
View File

@ -1,3 +1,4 @@
/*
#if GTEST
#include <gtest/gtest.h>
@ -220,3 +221,4 @@ TEST(Angle, MoveTowards) {
}
#endif
*/

2
test/DiscreteAngle_test.cc
View File

@ -1,3 +1,4 @@
/*
#if GTEST
#include <gtest/gtest.h>
@ -78,3 +79,4 @@ TEST(Angle16, Add) {
}
#endif
*/

2
test/Polar_test.cc
View File

@ -62,7 +62,7 @@ TEST(Polar, FromSpherical) {
p = Polar::FromSpherical(s);
EXPECT_FLOAT_EQ(p.distance, 1.0F) << "p.distance FromSpherical(-1 0 0)";
EXPECT_FLOAT_EQ(p.angle.InDegrees(), 180.0F)
EXPECT_FLOAT_EQ(p.angle.InDegrees(), -180.0F)
<< "p.angle FromSpherical(-1 0 0)";
s = Spherical(0, Angle::Degrees(0), Angle::Degrees(90));

2
test/Spherical_test.cc
View File

@ -71,7 +71,7 @@ TEST(Spherical, FromPolar) {
s = Spherical::FromPolar(p);
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)
<< "s.hor Polar(-1 0)";
EXPECT_FLOAT_EQ(s.direction.vertical.InDegrees(), 0.0F)
<< "s.vert Polar(-1 0)";