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

Spherical direction

Browse Source
This commit is contained in:
Pascal Serrarens 2024-09-24 11:48:25 +02:00
parent 136e44e000
commit 0ebfce48d7
12 changed files with 122 additions and 350 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

97
Angle16.cpp
View File

@ -1,97 +0,0 @@
// This Source Code Form is subject to the terms of the Mozilla Public
// 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 <stdlib.h>
#include "Angle.h"
template <>
AngleOf<signed short>::AngleOf(int angle) {
signed long long_angle = (signed short)angle * 65536;
this->value = (signed short)(long_angle / 360);
}
template <>
AngleOf<signed short>::AngleOf(float angle) {
if (!isfinite(angle)) {
value = 0;
return;
}
// map float [-180..180) to integer [-32768..32767]
this->value = (signed short)(angle / 360.0F * 65536.0F);
}
// template <>
// AngleOf<signed short>::operator float() const {
// float f = ((this->value * 180) / 32768.0F);
// return f;
// }
template <>
float AngleOf<signed short>::ToFloat() const {
float f = ((this->value * 180) / 32768.0F);
return f;
}
template <>
AngleOf<signed short> AngleOf<signed short>::operator-() const {
AngleOf<signed short> angle = AngleOf();
angle.value = -this->value;
return angle;
}
template <>
AngleOf<signed short> AngleOf<signed short>::operator-(
const AngleOf<signed short>& a) const {
AngleOf<signed short> angle = AngleOf();
angle.value = this->value - a.value;
return angle;
}
template <>
AngleOf<signed short> AngleOf<signed short>::operator+(
const AngleOf<signed short>& a) const {
AngleOf<signed short> angle = AngleOf();
angle.value = this->value + a.value;
return angle;
}
// Not correct!!! just for syntactical compilation ATM
template <>
AngleOf<signed short> AngleOf<signed short>::CosineRuleSide(
float a,
float b,
AngleOf<signed short> gamma) {
float a2 = a * a;
float b2 = b * b;
float d = a2 + b2 -
2 * a * b * cosf(gamma.ToFloat() * Passer::LinearAlgebra::Deg2Rad);
// Catch edge cases where float inacuracies lead tot nans
if (d < 0)
return 0.0f;
float c = sqrtf(d);
return c;
}
// Not correct!!! just for syntactical compilation ATM
template <>
AngleOf<signed short> AngleOf<signed short>::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) * Passer::LinearAlgebra::Rad2Deg;
return gamma;
}
*/

41
Angle16.h
View File

@ -1,41 +0,0 @@
// #include "AngleUsing.h"
/*
#include <math.h>
#include <stdlib.h>
#include "Angle.h"
namespace Passer {
namespace LinearAlgebra {
typedef AngleOf<signed short> Angle16;
// template <>
// inline static Angle16 Angle16::Degrees(short angle) {
// long long_angle = angle * 65535;
// long_angle = div(long_angle, (long)360);
// return Angle16(long_angle); //(long_angle / 360);
// }
// template <>
// inline static Angle16 Angle16::Degrees(float angle) {
// return (angle / 360.0F * 65536.0F);
// }
// template <> Angle16::AngleOf(float angle) {
// if (!isfinite(angle)) {
// value = 0;
// return;
// }
// // map float [-180..180) to integer [-32768..32767]
// this->value = (signed short)((angle / 360.0F) * 65536.0F);
// }
// template <> float Angle16::ToFloat() const {
// float f = ((this->value * 180) / 32768.0F);
// return f;
// }
} // namespace LinearAlgebra
} // namespace Passer
using namespace Passer::LinearAlgebra;
*/

31
Angle32.h
View File

@ -1,31 +0,0 @@
// #include "AngleUsing.h"
/*
#include <math.h>
#include "Angle.h"
namespace Passer {
namespace LinearAlgebra {
typedef AngleOf<signed long> Angle32;
// template <>
// Angle32::AngleOf(float angle) {
// if (!isfinite(angle)) {
// value = 0;
// return;
// }
// // map float [-180..180) to integer [-2147483648..2147483647]
// this->value = (signed long)((angle / 360.0F) * 4294967295.0F);
// }
// template <>
// float Angle32::ToFloat() const {
// float f = ((this->value * 180) / 2147483648.0F);
// return f;
// }
} // namespace LinearAlgebra
} // namespace Passer
using namespace Passer::LinearAlgebra;
*/

60
Angle8.cpp
View File

@ -1,60 +0,0 @@
// This Source Code Form is subject to the terms of the Mozilla Public
// 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 "Angle.h"
template <>
AngleOf<signed char>::AngleOf(int angle) {
signed short short_angle = (signed char)angle * 256;
this->value = (signed char)(short_angle / 360);
}
template <>
AngleOf<signed char>::AngleOf(float angle) {
if (!isfinite(angle)) {
value = 0;
return;
}
// map float [-180..180) to integer [-128..127]
float f = angle / 360.0F;
this->value = (signed char)(f * 256.0F);
}
// template <>
// AngleOf<signed char>::operator float() const {
// float f = (this->value * 180) / 128.0F;
// return f;
// }
template <>
float AngleOf<signed char>::ToFloat() const {
float f = (this->value * 180) / 128.0F;
return f;
}
template <>
AngleOf<signed char> AngleOf<signed char>::operator-() const {
AngleOf<signed char> angle = AngleOf();
angle.value = -this->value;
return angle;
}
template <>
AngleOf<signed char> AngleOf<signed char>::operator-(
const AngleOf<signed char>& a) const {
AngleOf<signed char> angle = AngleOf();
angle.value = this->value - a.value;
return angle;
}
template <>
AngleOf<signed char> AngleOf<signed char>::operator+(
const AngleOf<signed char>& a) const {
AngleOf<signed char> angle = AngleOf();
angle.value = this->value + a.value;
return angle;
}
*/

35
Angle8.h
View File

@ -1,35 +0,0 @@
// #include "AngleUsing.h"
/*
#include <math.h>
#include "Angle.h"
namespace Passer {
namespace LinearAlgebra {
typedef AngleOf<signed char> Angle8;
// template <>
// inline static Angle8 Angle8::Degrees(float angle) {
// return (angle / 360.0F * 256.0F);
// }
// template <> Angle8::AngleOf(float angle) {
// if (!isfinite(angle)) {
// value = 0;
// return;
// }
// // map float [-180..180) to integer [-128..127]
// float f = angle / 360.0F;
// this->value = (signed char)(f * 256.0F);
// }
// template <> float Angle8::ToFloat() const {
// float f = (this->value * 180) / 128.0F;
// return f;
// }
} // namespace LinearAlgebra
} // namespace Passer
using namespace Passer::LinearAlgebra;
*/

4
Polar.cpp
View File

@ -33,9 +33,9 @@ PolarOf<T> PolarOf<T>::FromVector2(Vector2 v) {
} }
template <typename T> template <typename T>
PolarOf<T> PolarOf<T>::FromSpherical(SphericalOf<T> v) { PolarOf<T> PolarOf<T>::FromSpherical(SphericalOf<T> v) {
float distance = v.distance * cosf(v.vertical.InDegrees() * float distance = v.distance * cosf(v.direction.vertical.InDegrees() *
Passer::LinearAlgebra::Deg2Rad); Passer::LinearAlgebra::Deg2Rad);
AngleOf<T> angle = v.horizontal; AngleOf<T> angle = v.direction.horizontal;
PolarOf<T> p = PolarOf(distance, angle); PolarOf<T> p = PolarOf(distance, angle);
return p; return p;
} }

43
Spherical.cpp
View File

@ -8,8 +8,9 @@
template <typename T> template <typename T>
SphericalOf<T>::SphericalOf() { SphericalOf<T>::SphericalOf() {
this->distance = 0.0f; this->distance = 0.0f;
this->horizontal = AngleOf<T>(); this->direction = DirectionOf<T>();
this->vertical = AngleOf<T>(); // this->horizontal = AngleOf<T>();
// this->vertical = AngleOf<T>();
} }
// template <> // template <>
@ -24,8 +25,15 @@ SphericalOf<T>::SphericalOf(float distance,
AngleOf<T> horizontal, AngleOf<T> horizontal,
AngleOf<T> vertical) { AngleOf<T> vertical) {
this->distance = distance; this->distance = distance;
this->horizontal = horizontal; this->direction = DirectionOf<T>(horizontal, vertical);
this->vertical = vertical; // this->horizontal = horizontal;
// this->vertical = vertical;
}
template <typename T>
SphericalOf<T>::SphericalOf(float distance, DirectionOf<T> direction) {
this->distance = distance;
this->direction = direction;
} }
// template <> // template <>
@ -70,8 +78,8 @@ SphericalOf<T> SphericalOf<T>::FromVector3(Vector3 v) {
template <typename T> template <typename T>
Vector3 SphericalOf<T>::ToVector3() const { Vector3 SphericalOf<T>::ToVector3() const {
float verticalRad = (pi / 2) - this->vertical.InRadians(); float verticalRad = (pi / 2) - this->direction.vertical.InRadians();
float horizontalRad = this->horizontal.InRadians(); float horizontalRad = this->direction.horizontal.InRadians();
float cosVertical = cosf(verticalRad); float cosVertical = cosf(verticalRad);
float sinVertical = sinf(verticalRad); float sinVertical = sinf(verticalRad);
@ -108,21 +116,17 @@ template <typename T>
const SphericalOf<T> SphericalOf<T>::down = const SphericalOf<T> SphericalOf<T>::down =
SphericalOf<T>(1.0f, AngleOf<T>(), AngleOf<T>::Degrees(-90)); SphericalOf<T>(1.0f, AngleOf<T>(), AngleOf<T>::Degrees(-90));
template <>
const SphericalOf<signed short> SphericalOf<signed short>::zero =
SphericalOf(0.0f, AngleOf<signed short>(), AngleOf<signed short>());
template <typename T> template <typename T>
SphericalOf<T> SphericalOf<T>::WithDistance(float distance) { SphericalOf<T> SphericalOf<T>::WithDistance(float distance) {
SphericalOf<T> v = SphericalOf<T>(distance, this->horizontal, this->vertical); SphericalOf<T> v = SphericalOf<T>(distance, this->direction);
return SphericalOf<T>(); return SphericalOf<T>();
} }
template <typename T> template <typename T>
SphericalOf<T> SphericalOf<T>::operator-() const { SphericalOf<T> SphericalOf<T>::operator-() const {
SphericalOf<T> v = SphericalOf<T>(this->distance, SphericalOf<T> v = SphericalOf<T>(
this->horizontal + AngleOf<T>::Degrees(180), this->distance, this->direction.horizontal + AngleOf<T>::Degrees(180),
this->vertical + AngleOf<T>::Degrees(180)); this->direction.vertical + AngleOf<T>::Degrees(180));
return v; return v;
} }
@ -266,20 +270,23 @@ template <typename T>
SphericalOf<T> SphericalOf<T>::Rotate(const SphericalOf<T>& v, SphericalOf<T> SphericalOf<T>::Rotate(const SphericalOf<T>& v,
AngleOf<T> horizontalAngle, AngleOf<T> horizontalAngle,
AngleOf<T> verticalAngle) { AngleOf<T> verticalAngle) {
SphericalOf<T> r = SphericalOf(v.distance, v.horizontal + horizontalAngle, SphericalOf<T> r =
v.vertical + verticalAngle); SphericalOf(v.distance, v.direction.horizontal + horizontalAngle,
v.direction.vertical + verticalAngle);
return r; return r;
} }
template <typename T> template <typename T>
SphericalOf<T> SphericalOf<T>::RotateHorizontal(const SphericalOf<T>& v, SphericalOf<T> SphericalOf<T>::RotateHorizontal(const SphericalOf<T>& v,
AngleOf<T> a) { AngleOf<T> a) {
SphericalOf<T> r = SphericalOf(v.distance, v.horizontal + a, v.vertical); SphericalOf<T> r =
SphericalOf(v.distance, v.direction.horizontal + a, v.direction.vertical);
return r; return r;
} }
template <typename T> template <typename T>
SphericalOf<T> SphericalOf<T>::RotateVertical(const SphericalOf<T>& v, SphericalOf<T> SphericalOf<T>::RotateVertical(const SphericalOf<T>& v,
AngleOf<T> a) { AngleOf<T> a) {
SphericalOf<T> r = SphericalOf(v.distance, v.horizontal, v.vertical + a); SphericalOf<T> r =
SphericalOf(v.distance, v.direction.horizontal, v.direction.vertical + a);
return r; return r;
} }

19
Spherical.h
View File

@ -5,8 +5,7 @@
#ifndef SPHERICAL_H #ifndef SPHERICAL_H
#define SPHERICAL_H #define SPHERICAL_H
#include "Angle.h" #include "Direction.h"
// #include "Polar.h"
namespace Passer { namespace Passer {
namespace LinearAlgebra { namespace LinearAlgebra {
@ -23,13 +22,15 @@ class SphericalOf {
float distance; float distance;
/// @brief The angle in the horizontal plane in degrees, clockwise rotation /// @brief The angle in the horizontal plane in degrees, clockwise rotation
/// @details The angle is automatically normalized to -180 .. 180 /// @details The angle is automatically normalized to -180 .. 180
AngleOf<T> horizontal; // AngleOf<T> horizontal;
/// @brief The angle in the vertical plane in degrees. Positive is upward. /// @brief The angle in the vertical plane in degrees. Positive is upward.
/// @details The angle is automatically normalized to -180 .. 180 /// @details The angle is automatically normalized to -180 .. 180
AngleOf<T> vertical; // AngleOf<T> vertical;
DirectionOf<T> direction;
SphericalOf<T>(); SphericalOf<T>();
SphericalOf<T>(float distance, AngleOf<T> horizontal, AngleOf<T> vertical); SphericalOf<T>(float distance, AngleOf<T> horizontal, AngleOf<T> vertical);
SphericalOf<T>(float distance, DirectionOf<T> direction);
static SphericalOf<T> FromPolar(PolarOf<T> v); static SphericalOf<T> FromPolar(PolarOf<T> v);
@ -76,11 +77,10 @@ class SphericalOf {
/// @remark This operation will scale the distance of the vector. The angle /// @remark This operation will scale the distance of the vector. The angle
/// will be unaffected. /// will be unaffected.
friend SphericalOf<T> operator*(const SphericalOf<T>& v, float f) { friend SphericalOf<T> operator*(const SphericalOf<T>& v, float f) {
return SphericalOf<T>(v.distance * f, v.horizontal, v.vertical); return SphericalOf<T>(v.distance * f, v.direction);
} }
friend SphericalOf<T> operator*(float f, const SphericalOf<T>& v) { friend SphericalOf<T> operator*(float f, const SphericalOf<T>& v) {
return SphericalOf<T>(v.distance * f, v.horizontal, return SphericalOf<T>(f * v.distance, v.direction);
v.vertical); // not correct, should be f * v.distance
} }
SphericalOf<T> operator*=(float f); SphericalOf<T> operator*=(float f);
/// @brief Scale the vector uniformly down /// @brief Scale the vector uniformly down
@ -89,11 +89,10 @@ class SphericalOf {
/// @remark This operation will scale the distance of the vector. The angle /// @remark This operation will scale the distance of the vector. The angle
/// will be unaffected. /// will be unaffected.
friend SphericalOf<T> operator/(const SphericalOf<T>& v, float f) { friend SphericalOf<T> operator/(const SphericalOf<T>& v, float f) {
return SphericalOf<T>(v.distance / f, v.horizontal, v.vertical); return SphericalOf<T>(v.distance / f, v.direction);
} }
friend SphericalOf<T> operator/(float f, const SphericalOf<T>& v) { friend SphericalOf<T> operator/(float f, const SphericalOf<T>& v) {
return SphericalOf<T>(v.distance / f, v.horizontal, return SphericalOf<T>(f / v.distance, v.direction);
v.vertical); // not correct, should be f / v.distance
} }
SphericalOf<T> operator/=(float f); SphericalOf<T> operator/=(float f);

6
SwingTwist.cpp
View File

@ -62,9 +62,9 @@ const SwingTwistOf<T> SwingTwistOf<T>::identity = SwingTwistOf();
template <typename T> template <typename T>
SphericalOf<T> SwingTwistOf<T>::operator*(const SphericalOf<T>& vector) const { SphericalOf<T> SwingTwistOf<T>::operator*(const SphericalOf<T>& vector) const {
SphericalOf<T> v = SphericalOf<T>(vector.distance, SphericalOf<T> v = SphericalOf<T>(
vector.horizontal + this->swing.horizontal, vector.distance, vector.direction.horizontal + this->swing.horizontal,
vector.vertical + this->swing.vertical); vector.direction.vertical + this->swing.vertical);
return v; return v;
} }

6
Vector3.cpp
View File

@ -31,10 +31,10 @@ Vector3::Vector3(Vector2 v) {
} }
Vector3::Vector3(Spherical s) { Vector3::Vector3(Spherical s) {
float verticalRad = float verticalRad = (90.0f - s.direction.vertical.InDegrees()) *
(90.0f - s.vertical.InDegrees()) * Passer::LinearAlgebra::Deg2Rad; Passer::LinearAlgebra::Deg2Rad;
float horizontalRad = float horizontalRad =
s.horizontal.InDegrees() * Passer::LinearAlgebra::Deg2Rad; s.direction.horizontal.InDegrees() * Passer::LinearAlgebra::Deg2Rad;
float cosVertical = cosf(verticalRad); float cosVertical = cosf(verticalRad);
float sinVertical = sinf(verticalRad); float sinVertical = sinf(verticalRad);
float cosHorizontal = cosf(horizontalRad); float cosHorizontal = cosf(horizontalRad);

58
test/Spherical16_test.cc
View File

@ -13,22 +13,24 @@ TEST(Spherical16, FromVector3) {
Spherical16 s = Spherical16::FromVector3(v); Spherical16 s = Spherical16::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((float)s.horizontal.InDegrees(), 0.0F) << "s.hor 0 0 1"; EXPECT_FLOAT_EQ((float)s.direction.horizontal.InDegrees(), 0.0F)
EXPECT_FLOAT_EQ((float)s.vertical.InDegrees(), 0.0F) << "s.vert 0 0 1"; << "s.hor 0 0 1";
EXPECT_FLOAT_EQ((float)s.direction.vertical.InDegrees(), 0.0F)
<< "s.vert 0 0 1";
v = Vector3(0, 1, 0); v = Vector3(0, 1, 0);
s = Spherical16::FromVector3(v); s = Spherical16::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.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.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 = Spherical16::FromVector3(v); s = Spherical16::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.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.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, FromPolar) { // TEST(Spherical16, FromPolar) {
@ -75,10 +77,14 @@ TEST(Spherical16, Incident1) {
Spherical16 sr = Spherical16 sr =
Spherical16(2.49F, Angle16::Degrees(98.18f), Angle16::Degrees(24.4F)); Spherical16(2.49F, Angle16::Degrees(98.18f), Angle16::Degrees(24.4F));
EXPECT_NEAR(s.distance, sr.distance, 1.0e-01); EXPECT_NEAR(s.distance, sr.distance, 1.0e-01);
EXPECT_NEAR(s.horizontal.InDegrees(), sr.horizontal.InDegrees(), 1.0e-02); EXPECT_NEAR(s.direction.horizontal.InDegrees(),
EXPECT_NEAR(s.vertical.InDegrees(), sr.vertical.InDegrees(), 1.0e-02); sr.direction.horizontal.InDegrees(), 1.0e-02);
EXPECT_NEAR(s.direction.vertical.InDegrees(),
sr.direction.vertical.InDegrees(), 1.0e-02);
Vector3 r = Spherical16(sr.distance, sr.horizontal, sr.vertical).ToVector3(); Vector3 r =
Spherical16(sr.distance, sr.direction.horizontal, sr.direction.vertical)
.ToVector3();
EXPECT_NEAR(r.Right(), v.Right(), 1.0e-02) << "toVector3.x 1 0 0"; EXPECT_NEAR(r.Right(), v.Right(), 1.0e-02) << "toVector3.x 1 0 0";
EXPECT_NEAR(r.Up(), v.Up(), 1.0e-02) << "toVector3.y 1 0 0"; EXPECT_NEAR(r.Up(), v.Up(), 1.0e-02) << "toVector3.y 1 0 0";
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";
@ -91,10 +97,14 @@ TEST(Spherical16, Incident2) {
Spherical16 sr = Spherical16(1.4142135623F, Angle16::Degrees(45.0f), Spherical16 sr = Spherical16(1.4142135623F, Angle16::Degrees(45.0f),
Angle16::Degrees(0.0F)); Angle16::Degrees(0.0F));
EXPECT_NEAR(s.distance, sr.distance, 1.0e-05); EXPECT_NEAR(s.distance, sr.distance, 1.0e-05);
EXPECT_NEAR(s.horizontal.InDegrees(), sr.horizontal.InDegrees(), 1.0e-05); EXPECT_NEAR(s.direction.horizontal.InDegrees(),
EXPECT_NEAR(s.vertical.InDegrees(), sr.vertical.InDegrees(), 1.0e-05); sr.direction.horizontal.InDegrees(), 1.0e-05);
EXPECT_NEAR(s.direction.vertical.InDegrees(),
sr.direction.vertical.InDegrees(), 1.0e-05);
Vector3 r = Spherical16(sr.distance, sr.horizontal, sr.vertical).ToVector3(); Vector3 r =
Spherical16(sr.distance, sr.direction.horizontal, sr.direction.vertical)
.ToVector3();
EXPECT_NEAR(r.Right(), v.Right(), 1.0e-06); EXPECT_NEAR(r.Right(), v.Right(), 1.0e-06);
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);
@ -104,21 +114,25 @@ TEST(Spherical16, Incident2) {
sr = Spherical16(1.4142135623F, Angle16::Degrees(0), Angle16::Degrees(45)); sr = Spherical16(1.4142135623F, Angle16::Degrees(0), Angle16::Degrees(45));
EXPECT_NEAR(s.distance, sr.distance, 1.0e-05); EXPECT_NEAR(s.distance, sr.distance, 1.0e-05);
EXPECT_NEAR(s.horizontal.InDegrees(), sr.horizontal.InDegrees(), 1.0e-05); EXPECT_NEAR(s.direction.horizontal.InDegrees(),
EXPECT_NEAR(s.vertical.InDegrees(), sr.vertical.InDegrees(), 1.0e-05); sr.direction.horizontal.InDegrees(), 1.0e-05);
EXPECT_NEAR(s.direction.vertical.InDegrees(),
sr.direction.vertical.InDegrees(), 1.0e-05);
r = Spherical16(sr.distance, sr.horizontal, sr.vertical).ToVector3(); r = Spherical16(sr.distance, sr.direction.horizontal, sr.direction.vertical)
.ToVector3();
EXPECT_NEAR(r.Right(), v.Right(), 1.0e-06); EXPECT_NEAR(r.Right(), v.Right(), 1.0e-06);
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);
v = Vector3(1.0f, 1.0f, 1.0f); v = Vector3(1.0f, 1.0f, 1.0f);
s = Spherical16::FromVector3(v); s = Spherical16::FromVector3(v);
r = Spherical16(s.distance, s.horizontal, s.vertical).ToVector3(); r = Spherical16(s.distance, s.direction.horizontal, s.direction.vertical)
.ToVector3();
EXPECT_NEAR(s.distance, 1.73205080F, 1.0e-02); EXPECT_NEAR(s.distance, 1.73205080F, 1.0e-02);
EXPECT_NEAR(s.horizontal.InDegrees(), 45.0F, 1.0e-02); EXPECT_NEAR(s.direction.horizontal.InDegrees(), 45.0F, 1.0e-02);
EXPECT_NEAR(s.vertical.InDegrees(), 35.26F, 1.0e-02); EXPECT_NEAR(s.direction.vertical.InDegrees(), 35.26F, 1.0e-02);
EXPECT_NEAR(r.Right(), v.Right(), 1.0e-04); EXPECT_NEAR(r.Right(), v.Right(), 1.0e-04);
EXPECT_NEAR(r.Up(), v.Up(), 1.0e-04); EXPECT_NEAR(r.Up(), v.Up(), 1.0e-04);
@ -146,14 +160,14 @@ TEST(Spherical16, Addition) {
v2 = Spherical16(1, Angle16::Degrees(-45), Angle16::Degrees(0)); v2 = Spherical16(1, Angle16::Degrees(-45), Angle16::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.horizontal.InDegrees(), 0) << "Addition(1 -45 0)"; EXPECT_FLOAT_EQ(r.direction.horizontal.InDegrees(), 0) << "Addition(1 -45 0)";
EXPECT_FLOAT_EQ(r.vertical.InDegrees(), 0) << "Addition(1 -45 0)"; EXPECT_FLOAT_EQ(r.direction.vertical.InDegrees(), 0) << "Addition(1 -45 0)";
v2 = Spherical16(1, Angle16::Degrees(0), Angle16::Degrees(90)); v2 = Spherical16(1, Angle16::Degrees(0), Angle16::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.horizontal.InDegrees(), 45) << "Addition(1 0 90)"; EXPECT_FLOAT_EQ(r.direction.horizontal.InDegrees(), 45) << "Addition(1 0 90)";
EXPECT_FLOAT_EQ(r.vertical.InDegrees(), 45) << "Addition(1 0 90)"; EXPECT_FLOAT_EQ(r.direction.vertical.InDegrees(), 45) << "Addition(1 0 90)";
} }
#endif #endif

72
test/Spherical_test.cc
View File

@ -12,22 +12,22 @@ TEST(Spherical, FromVector3) {
Spherical s = Spherical::FromVector3(v); Spherical s = Spherical::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.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.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 = Spherical::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.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.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 = Spherical::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.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.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(Spherical, FromPolar) {
@ -35,36 +35,46 @@ TEST(Spherical, FromPolar) {
Spherical s = Spherical::FromPolar(p); Spherical s = Spherical::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.horizontal.InDegrees(), 0.0F) << "s.hor Polar(1 0)"; EXPECT_FLOAT_EQ(s.direction.horizontal.InDegrees(), 0.0F)
EXPECT_FLOAT_EQ(s.vertical.InDegrees(), 0.0F) << "s.vert Polar(1 0)"; << "s.hor Polar(1 0)";
EXPECT_FLOAT_EQ(s.direction.vertical.InDegrees(), 0.0F)
<< "s.vert Polar(1 0)";
p = Polar(1, Angle::Degrees(45)); p = Polar(1, Angle::Degrees(45));
s = Spherical::FromPolar(p); s = Spherical::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.horizontal.InDegrees(), 45.0F) << "s.hor Polar(1 45)"; EXPECT_FLOAT_EQ(s.direction.horizontal.InDegrees(), 45.0F)
EXPECT_FLOAT_EQ(s.vertical.InDegrees(), 0.0F) << "s.vert Polar(1 45)"; << "s.hor Polar(1 45)";
EXPECT_FLOAT_EQ(s.direction.vertical.InDegrees(), 0.0F)
<< "s.vert Polar(1 45)";
p = Polar(1, Angle::Degrees(-45)); p = Polar(1, Angle::Degrees(-45));
s = Spherical::FromPolar(p); s = Spherical::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.horizontal.InDegrees(), -45.0F) << "s.hor Polar(1 -45)"; EXPECT_FLOAT_EQ(s.direction.horizontal.InDegrees(), -45.0F)
EXPECT_FLOAT_EQ(s.vertical.InDegrees(), 0.0F) << "s.vert Polar(1 -45)"; << "s.hor Polar(1 -45)";
EXPECT_FLOAT_EQ(s.direction.vertical.InDegrees(), 0.0F)
<< "s.vert Polar(1 -45)";
p = Polar(0, Angle::Degrees(0)); p = Polar(0, Angle::Degrees(0));
s = Spherical::FromPolar(p); s = Spherical::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.horizontal.InDegrees(), 0.0F) << "s.hor Polar(0 0)"; EXPECT_FLOAT_EQ(s.direction.horizontal.InDegrees(), 0.0F)
EXPECT_FLOAT_EQ(s.vertical.InDegrees(), 0.0F) << "s.vert Polar(0 0)"; << "s.hor Polar(0 0)";
EXPECT_FLOAT_EQ(s.direction.vertical.InDegrees(), 0.0F)
<< "s.vert Polar(0 0)";
p = Polar(-1, Angle::Degrees(0)); p = Polar(-1, Angle::Degrees(0));
s = Spherical::FromPolar(p); s = Spherical::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.horizontal.InDegrees(), 180.0F) << "s.hor Polar(-1 0)"; EXPECT_FLOAT_EQ(s.direction.horizontal.InDegrees(), 180.0F)
EXPECT_FLOAT_EQ(s.vertical.InDegrees(), 0.0F) << "s.vert Polar(-1 0)"; << "s.hor Polar(-1 0)";
EXPECT_FLOAT_EQ(s.direction.vertical.InDegrees(), 0.0F)
<< "s.vert Polar(-1 0)";
} }
TEST(Spherical, Incident1) { TEST(Spherical, Incident1) {
@ -74,8 +84,10 @@ TEST(Spherical, Incident1) {
Spherical sr = Spherical sr =
Spherical(2.49F, Angle::Degrees(98.18f), Angle::Degrees(24.4F)); Spherical(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.horizontal.InDegrees(), sr.horizontal.InDegrees(), 1.0e-02); EXPECT_NEAR(s.direction.horizontal.InDegrees(),
EXPECT_NEAR(s.vertical.InDegrees(), sr.vertical.InDegrees(), 1.0e-02); sr.direction.horizontal.InDegrees(), 1.0e-02);
EXPECT_NEAR(s.direction.vertical.InDegrees(),
sr.direction.vertical.InDegrees(), 1.0e-02);
Vector3 r = Vector3(sr); Vector3 r = Vector3(sr);
EXPECT_NEAR(r.Right(), v.Right(), 1.0e-02) << "toVector3.x 1 0 0"; EXPECT_NEAR(r.Right(), v.Right(), 1.0e-02) << "toVector3.x 1 0 0";
@ -90,8 +102,10 @@ TEST(Spherical, Incident2) {
Spherical sr = Spherical sr =
Spherical(1.4142135623F, Angle::Degrees(45.0f), Angle::Degrees(0.0F)); Spherical(1.4142135623F, Angle::Degrees(45.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.horizontal.InDegrees(), sr.horizontal.InDegrees(), 1.0e-05); EXPECT_NEAR(s.direction.horizontal.InDegrees(),
EXPECT_NEAR(s.vertical.InDegrees(), sr.vertical.InDegrees(), 1.0e-05); sr.direction.horizontal.InDegrees(), 1.0e-05);
EXPECT_NEAR(s.direction.vertical.InDegrees(),
sr.direction.vertical.InDegrees(), 1.0e-05);
Vector3 r = Vector3(sr); Vector3 r = Vector3(sr);
EXPECT_NEAR(r.Right(), v.Right(), 1.0e-06); EXPECT_NEAR(r.Right(), v.Right(), 1.0e-06);
@ -103,8 +117,10 @@ TEST(Spherical, Incident2) {
sr = Spherical(1.4142135623F, Angle::Degrees(0.0f), Angle::Degrees(45.0F)); sr = Spherical(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.horizontal.InDegrees(), sr.horizontal.InDegrees(), 1.0e-05); EXPECT_NEAR(s.direction.horizontal.InDegrees(),
EXPECT_NEAR(s.vertical.InDegrees(), sr.vertical.InDegrees(), 1.0e-05); sr.direction.horizontal.InDegrees(), 1.0e-05);
EXPECT_NEAR(s.direction.vertical.InDegrees(),
sr.direction.vertical.InDegrees(), 1.0e-05);
r = Vector3(sr); r = Vector3(sr);
EXPECT_NEAR(r.Right(), v.Right(), 1.0e-06); EXPECT_NEAR(r.Right(), v.Right(), 1.0e-06);
@ -116,8 +132,8 @@ TEST(Spherical, Incident2) {
r = Vector3(s); r = Vector3(s);
EXPECT_NEAR(s.distance, 1.73205080F, 1.0e-02); EXPECT_NEAR(s.distance, 1.73205080F, 1.0e-02);
EXPECT_NEAR(s.horizontal.InDegrees(), 45.0F, 1.0e-02); EXPECT_NEAR(s.direction.horizontal.InDegrees(), 45.0F, 1.0e-02);
EXPECT_NEAR(s.vertical.InDegrees(), 35.26F, 1.0e-02); EXPECT_NEAR(s.direction.vertical.InDegrees(), 35.26F, 1.0e-02);
EXPECT_NEAR(r.Right(), v.Right(), 1.0e-06); EXPECT_NEAR(r.Right(), v.Right(), 1.0e-06);
EXPECT_NEAR(r.Up(), v.Up(), 1.0e-06); EXPECT_NEAR(r.Up(), v.Up(), 1.0e-06);
@ -145,14 +161,14 @@ TEST(Spherical, Addition) {
v2 = Spherical(1, Angle::Degrees(-45), Angle::Degrees(0)); v2 = Spherical(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.horizontal.InDegrees(), 0) << "Addition(1 -45 0)"; EXPECT_FLOAT_EQ(r.direction.horizontal.InDegrees(), 0) << "Addition(1 -45 0)";
EXPECT_FLOAT_EQ(r.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 = Spherical(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.horizontal.InDegrees(), 45) << "Addition(1 0 90)"; EXPECT_FLOAT_EQ(r.direction.horizontal.InDegrees(), 45) << "Addition(1 0 90)";
EXPECT_FLOAT_EQ(r.vertical.InDegrees(), 45) << "Addition(1 0 90)"; EXPECT_FLOAT_EQ(r.direction.vertical.InDegrees(), 45) << "Addition(1 0 90)";
} }
#endif #endif