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

Improved unit tests

Browse Source
This commit is contained in:
Pascal Serrarens 2024-05-12 15:32:16 +02:00
parent 6ddb074225
commit b1e34b619e
5 changed files with 123 additions and 108 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

66
Polar.cpp
View File

@ -5,56 +5,54 @@
#include "Spherical.h"
Polar::Polar() {
angle = 0.0F;
distance = 0.0F;
this->distance = 0.0f;
this->angle = 0.0f;
}
Polar::Polar(float distance, Angle angle) {
// distance should always be 0 or greater
if (distance < 0) {
if (distance < 0.0f) {
this->distance = -distance;
this->angle = Angle::Normalize(angle - 180);
} else {
this->distance = distance;
this->angle = Angle::Normalize(angle);
if (this->distance == 0.0f)
// angle is always 0 if distance is 0
this->angle = 0.0f;
else
this->angle = Angle::Normalize(angle);
}
}
Polar::Polar(Vector2 v) {
angle = Vector2::SignedAngle(
Vector2::forward,
v); // atan2(v.x, sqrt(v.z * v.z + v.y * v.y)) * Angle::Rad2Deg;
distance = v.magnitude();
this->distance = v.magnitude();
this->angle = Vector2::SignedAngle(Vector2::forward, v);
}
Polar::Polar(Spherical s) {
angle = s.horizontalAngle;
distance = s.distance * cosf(s.verticalAngle * Angle::Deg2Rad);
Polar::Polar(Spherical v) {
this->distance = v.distance * cosf(v.verticalAngle * Angle::Deg2Rad);
this->angle = v.horizontalAngle;
}
const Polar Polar::zero = Polar(0, 0);
const Polar Polar::zero = Polar(0.0f, 0.0f);
float Polar::Distance(Polar &v1, Polar &v2) {
float d = Angle::CosineRuleSide(v1.distance, v2.distance,
(float)v2.angle - (float)v1.angle);
return d;
bool Polar::operator==(const Polar &v) {
return (this->distance == v.distance && this->angle == v.angle);
}
Polar Polar::operator+(Polar &v2) {
if (v2.distance == 0)
return Polar(this->distance,
this->angle); // Polar(this->angle, this->distance);
if (this->distance == 0)
return Polar(this->distance, this->angle);
if (this->distance == 0.0f)
return v2;
float deltaAngle = Angle::Normalize(v2.angle - (float)this->angle);
float rotation = deltaAngle < 0 ? 180 + deltaAngle : 180 - deltaAngle;
float rotation =
deltaAngle < 0.0f ? 180.0f + deltaAngle : 180.0f - deltaAngle;
if (rotation == 180 && v2.distance > 0) {
if (rotation == 180.0f && v2.distance > 0.0f) {
// angle is too small, take this angle and add the distances
return Polar(
this->distance + v2.distance,
this->angle); // Polar(this->angle, this->distance + v2.distance);
return Polar(this->distance + v2.distance, this->angle);
}
float newDistance =
@ -63,24 +61,22 @@ Polar Polar::operator+(Polar &v2) {
float angle =
Angle::CosineRuleAngle(newDistance, this->distance, v2.distance);
float newAngle =
deltaAngle < 0 ? (float)this->angle - angle : (float)this->angle + angle;
float newAngle = deltaAngle < 0.0f ? (float)this->angle - angle
: (float)this->angle + angle;
newAngle = Angle::Normalize(newAngle);
Polar vector = Polar(newDistance, newAngle); // Polar(newAngle, newDistance);
Polar vector = Polar(newDistance, newAngle);
return vector;
}
Polar Polar::operator-() {
Polar vector = Polar(this->distance,
(float)this->angle -
180); // Polar(this->angle - 180, this->distance);
return vector;
Polar v = Polar(this->distance, this->angle + 180);
return v;
}
Polar Polar::operator-(Polar &v2) {
// Polar vector = *this + Polar(v2.distance, (float)v2.angle - 180);
//(Polar(v2.angle - 180, v2.distance));
Polar vector = -v2;
v2 = -v2;
return *this + v2;
}
@ -94,6 +90,12 @@ Polar Polar::operator/(const float &f) {
this->angle); // Polar(this->angle, this->distance / f);
}
float Polar::Distance(Polar &v1, Polar &v2) {
float d = Angle::CosineRuleSide(v1.distance, v2.distance,
(float)v2.angle - (float)v1.angle);
return d;
}
Polar Polar::Rotate(Polar v, Angle angle) {
v.angle = Angle::Normalize(v.angle + angle);
return v;

61
Polar.h
View File

@ -19,57 +19,48 @@ struct Spherical;
/// reference direction and a distance.
struct Polar {
public:
/// <summary>
/// The angle in degrees, clockwise rotation
/// </summary>
/// The angle is normalized to -180 .. 180
Angle angle;
/// <summary>
/// The distance in meters
/// </summary>
/// The distance should never be negative
/// @brief The distance in meters
/// @remark The distance shall never be negative
float distance;
/// @brief The angle in degrees clockwise rotation
/// @remark The angle shall be between -180 .. 180
Angle angle;
/// <summary>
/// Create a new polar vector with zero degrees and distance
/// </summary>
/// @brief A new vector with polar coordinates with zero degrees and distance
Polar();
/// <summary>
/// Create a new polar vector
/// </summary>
/// <param name="angle">The angle in degrees, clockwise rotation</param>
/// <param name="distance">The distance in meters</param>
// Polar(float angle, float distance);
/// @brief A new vector with polar coordinates
/// @param distance The distance in meters
/// @param angle The angle in degrees, clockwise rotation
/// @note The distance is automatically converted to a positive value.
/// @note The angle is automatically normalized to -180 .. 180
Polar(float distance, Angle angle);
/// <summary>
/// Convert a Vector2 to a Polar coordinate
/// </summary>
/// <param name="v">The 2D carthesian vector</param>
/// @brief Convert a vector from 2D carthesian coordinates to polar
/// coordinates
/// @param v The vector to convert
Polar(Vector2 v);
/// <summary>
/// Convert a Spherical coordinate to a Polar coordinate
/// </summary>
/// <param name="s">The spherical coordinate</param>
/// @brief Convert a vector from spherical coordinates to polar coordinates
/// @param s The vector to convert
/// @note The resulting vector will be projected on the horizontal plane
Polar(Spherical s);
/// <summary>
/// A polar vector with zero degrees and distance
/// </summary>
/// @brief A polar vector with zero degrees and distance
const static Polar zero;
/// <summary>
/// Negate the polar vector.
/// </summary>
bool operator==(const Polar &v);
/// @brief Negate the vector
/// @return The negated vector
/// This will rotate the vector by 180 degrees. Distance will stay the same.
/// <returns>The negated vector</returns>
Polar operator-();
/// <summary>
/// Substract a polar vector from this coordinate
/// </summary>
/// <param name="v">The vector to subtract from this vector</param>
/// <returns>The result of the subtraction</returns>
/// @brief Subtract a polar vector from this vector
/// @param v The vector to subtract
/// @return The result of the subtraction
Polar operator-(Polar &v);
/// <summary>

49
Spherical.cpp
View File

@ -5,18 +5,16 @@
#include <math.h>
// using Angle = float;
Spherical::Spherical() {
this->horizontalAngle = 0;
this->verticalAngle = 0;
this->distance = 0;
this->distance = 0.0f;
this->horizontalAngle = 0.0f;
this->verticalAngle = 0.0f;
}
Spherical::Spherical(Polar polar) {
this->horizontalAngle = polar.angle;
this->verticalAngle = 0.0F;
this->distance = polar.distance;
this->horizontalAngle = polar.angle;
this->verticalAngle = 0.0f;
}
Spherical::Spherical(float distance, Angle horizontalAngle,
@ -33,17 +31,24 @@ Spherical::Spherical(float distance, Angle horizontalAngle,
}
Spherical::Spherical(Vector3 v) {
distance = v.magnitude();
this->distance = v.magnitude();
if (distance == 0.0f) {
verticalAngle = 0;
horizontalAngle = 0;
this->verticalAngle = 0.0f;
this->horizontalAngle = 0.0f;
} else {
verticalAngle = (90 - acosf(v.y / distance) * Angle::Rad2Deg);
horizontalAngle = atan2f(v.x, v.z) * Angle::Rad2Deg;
this->verticalAngle =
(90.0f - acosf(v.y / this->distance) * Angle::Rad2Deg);
this->horizontalAngle = atan2f(v.x, v.z) * Angle::Rad2Deg;
}
}
const Spherical Spherical::zero = Spherical(0.0F, (Angle)0.0F, (Angle)0.0F);
const Spherical Spherical::zero = Spherical(0.0f, 0.0f, 0.0f);
Spherical Spherical::operator-() {
Spherical v = Spherical(this->distance, this->horizontalAngle + 180.0f,
this->verticalAngle + 180.0f);
return v;
}
// float Spherical::GetSwing() {
// // Not sure if this is correct
@ -51,19 +56,7 @@ const Spherical Spherical::zero = Spherical(0.0F, (Angle)0.0F, (Angle)0.0F);
// verticalAngle * verticalAngle);
// }
// Polar Spherical::ProjectOnHorizontalPlane() {
// return Polar(horizontalAngle, distance);
// }
// Vector3 Spherical::ToVector3() {
// float verticalRad = (90 - verticalAngle) * Angle::Deg2Rad;
// float horizontalRad = horizontalAngle * Angle::Deg2Rad;
// float cosVertical = cosf(verticalRad);
// float sinVertical = sinf(verticalRad);
// float cosHorizontal = cosf(horizontalRad);
// float sinHorizontal = sinf(horizontalRad);
// Vector3 v = Vector3(this->distance * sinVertical * sinHorizontal,
// this->distance * cosVertical,
// this->distance * sinVertical * cosHorizontal);
// return v;
// float Spherical::Distance(const Spherical &s1, const Spherical &s2) {
// float d = 0;
// return d;
// }

31
Spherical.h
View File

@ -1,4 +1,3 @@
/// @copyright
/// 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/.
@ -21,8 +20,9 @@ struct Vector3;
/// as a forward direction.
struct Spherical {
public:
/// @brief The distance in meters
/// @remark The distance should never be negative
float distance;
/// @brief The angle in the horizontal plane in degrees, clockwise rotation
/// @details The angle is automatically normalized to -180 .. 180
Angle horizontalAngle;
@ -33,18 +33,15 @@ public:
/// @brief Create a new spherical vector with zero degrees and distance
Spherical();
/// @brief Create a new spherical vector
/// @param polarAngle The angle in the horizontal plane in degrees,
/// clockwise rotation
/// @param elevationAngle The angle in the vertical plan in degrees,
/// zero is forward, positive is upward
/// @param distance The distance in meters
// Spherical(float polarAngle, float elevationAngle, float distance);
/// @param horizontalAngle The angle in the horizontal plane in degrees,
/// clockwise rotation
/// @param verticalAngle The angle in the vertical plan in degrees,
/// zero is forward, positive is upward
Spherical(float distance, Angle horizontalAngle, Angle verticalAngle);
/// @brief Convert polar coordinates to spherical coordinates
/// @param polar The polar coordinate
Spherical(Polar polar);
/// @brief Convert 3D carthesian coordinates to spherical coordinates
/// @param v Vector in 3D carthesian coordinates;
Spherical(Vector3 v);
@ -52,11 +49,19 @@ public:
/// @brief A spherical vector with zero degree angles and distance
const static Spherical zero;
// float GetSwing();
/// @brief Negate the vector
/// @return The negated vector
/// This will rotate the vector by 180 degrees horizontally and
/// vertically. Distance will stay the same.
Spherical operator-();
// Polar ProjectOnHorizontalPlane();
// Vector3 ToVector3();
/// <summary>
/// The distance between two vectors
/// </summary>
/// <param name="v1">The first vector</param>
/// <param name="v2">The second vector</param>
/// <returns>The distance between the two vectors</returns>
// static float Distance(const Spherical &s1, const Spherical &s2);
};
} // namespace Passer

24
test/Polar_test.cc
View File

@ -68,4 +68,28 @@ TEST(Polar, FromSpherical) {
EXPECT_FLOAT_EQ(p.angle, 0.0F) << "p.angle FromSpherical(0 0 90)";
}
TEST(Polar, Negate) {
Polar v = Polar(2, 45);
Polar r = Polar::zero;
r = -v;
EXPECT_FLOAT_EQ(r.distance, 2);
EXPECT_FLOAT_EQ(r.angle, -135);
EXPECT_TRUE(r == Polar(2, -135)) << "Negate(2 45)";
v = Polar(2, -45);
r = -v;
EXPECT_TRUE(r == Polar(2, 135)) << "Negate(2 -45)";
v = Polar(2, 0);
r = -v;
EXPECT_TRUE(r == Polar(2, 180)) << "Negate(2 0)";
v = Polar(0, 0);
r = -v;
EXPECT_FLOAT_EQ(r.distance, 0.0f);
EXPECT_FLOAT_EQ(r.angle, 0.0f);
EXPECT_TRUE(r == Polar(0, 0)) << "Negate(0 0)";
}
#endif