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

Extend ulitity functions

Browse Source
This commit is contained in:
Pascal Serrarens 2024-05-13 19:01:21 +02:00
parent 95713c8621
commit 62d74396e1
9 changed files with 227 additions and 81 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

76
Polar.cpp
View File

@ -8,7 +8,6 @@ Polar::Polar() {
this->distance = 0.0f;
this->angle = 0.0f;
}
Polar::Polar(float distance, Angle angle) {
// distance should always be 0 or greater
if (distance < 0.0f) {
@ -23,20 +22,22 @@ Polar::Polar(float distance, Angle angle) {
this->angle = Angle::Normalize(angle);
}
}
Polar::Polar(Vector2 v) {
this->distance = v.magnitude();
this->angle = Vector2::SignedAngle(Vector2::forward, v);
}
Polar::Polar(Spherical v) {
this->distance = v.distance * cosf(v.verticalAngle * Angle::Deg2Rad);
this->angle = v.horizontalAngle;
}
const Polar Polar::zero = Polar(0.0f, 0.0f);
const Polar Polar::forward = Polar(1.0f, 0.0f);
const Polar Polar::back = Polar(1.0, 180.0f);
const Polar Polar::right = Polar(1.0, 90.0f);
const Polar Polar::left = Polar(1.0, -90.0f);
bool Polar::operator==(const Polar &v) {
bool Polar::operator==(const Polar &v) const {
return (this->distance == v.distance && this->angle == v.angle);
}
@ -49,26 +50,38 @@ Polar Polar::normalized() const {
return r;
}
Polar Polar::operator+(Polar &v2) {
if (v2.distance == 0)
Polar Polar::operator-() const {
Polar v = Polar(this->distance, this->angle + 180);
return v;
}
Polar Polar::operator-(const Polar &v) const {
Polar r = -v;
return *this + r;
}
Polar Polar::operator-=(const Polar &v) {
*this = *this - v;
return *this;
}
Polar Polar::operator+(const Polar &v) const {
if (v.distance == 0)
return Polar(this->distance, this->angle);
if (this->distance == 0.0f)
return v2;
return v;
float deltaAngle = Angle::Normalize(v2.angle - (float)this->angle);
float deltaAngle = Angle::Normalize(v.angle - (float)this->angle);
float rotation =
deltaAngle < 0.0f ? 180.0f + deltaAngle : 180.0f - deltaAngle;
if (rotation == 180.0f && v2.distance > 0.0f) {
if (rotation == 180.0f && v.distance > 0.0f) {
// angle is too small, take this angle and add the distances
return Polar(this->distance + v2.distance, this->angle);
return Polar(this->distance + v.distance, this->angle);
}
float newDistance =
Angle::CosineRuleSide(v2.distance, this->distance, rotation);
Angle::CosineRuleSide(v.distance, this->distance, rotation);
float angle =
Angle::CosineRuleAngle(newDistance, this->distance, v2.distance);
float angle = Angle::CosineRuleAngle(newDistance, this->distance, v.distance);
float newAngle = deltaAngle < 0.0f ? (float)this->angle - angle
: (float)this->angle + angle;
@ -76,27 +89,30 @@ Polar Polar::operator+(Polar &v2) {
Polar vector = Polar(newDistance, newAngle);
return vector;
}
Polar Polar::operator-() {
Polar v = Polar(this->distance, this->angle + 180);
return v;
Polar Polar::operator+=(const Polar &v) {
*this = *this + v;
return *this;
}
Polar Polar::operator-(Polar &v2) {
// Polar vector = *this + Polar(v2.distance, (float)v2.angle - 180);
//(Polar(v2.angle - 180, v2.distance));
v2 = -v2;
return *this + v2;
Polar Passer::operator*(const Polar &v, float f) {
return Polar(v.distance * f, v.angle);
}
Polar Polar::operator*(float f) const {
return Polar(this->distance * f,
this->angle); // Polar(this->angle, this->distance * f);
Polar Passer::operator*(float f, const Polar &v) {
return Polar(v.distance * f, v.angle);
}
Polar Polar::operator/(const float &f) {
return Polar(this->distance / f,
this->angle); // Polar(this->angle, this->distance / f);
Polar Polar::operator*=(float f) {
this->distance *= f;
return *this;
}
Polar Passer::operator/(const Polar &v, float f) {
return Polar(v.distance / f, v.angle);
}
Polar Passer::operator/(float f, const Polar &v) {
return Polar(v.distance / f, v.angle);
}
Polar Polar::operator/=(float f) {
this->distance /= f;
return *this;
}
float Polar::Distance(const Polar &v1, const Polar &v2) {

33
Polar.h
View File

@ -13,8 +13,8 @@ struct Vector2;
struct Spherical;
/// @brief A polar vector
/// This will use the polar coordinate system consisting of a angle from a
/// reference direction and a distance.
/// @details This will use the polar coordinate system consisting of a angle
/// from a reference direction and a distance.
struct Polar {
public:
/// @brief The distance in meters
@ -36,7 +36,6 @@ public:
/// coordinates
/// @param v The vector to convert
Polar(Vector2 v);
/// @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
@ -44,13 +43,21 @@ public:
/// @brief A polar vector with zero degrees and distance
const static Polar zero;
/// @brief A normalized forward-oriented vector
const static Polar forward;
/// @brief A normalized back-oriented vector
const static Polar back;
/// @brief A normalized right-oriented vector
const static Polar right;
/// @brief A normalized left-oriented vector
const static Polar left;
/// @brief Equality test to another vector
/// @param v The vector to check against
/// @return true: if it is identical to the given vector
/// @note This uses float comparison to check equality which may have strange
/// effects. Equality on floats should be avoided.
bool operator==(const Polar &v);
bool operator==(const Polar &v) const;
/// @brief The vector length
/// @param v The vector for which you need the length
@ -71,27 +78,35 @@ public:
/// @brief Negate the vector
/// @return The negated vector
/// This will rotate the vector by 180 degrees. Distance will stay the same.
Polar operator-();
Polar operator-() const;
/// @brief Subtract a polar vector from this vector
/// @param v The vector to subtract
/// @return The result of the subtraction
Polar operator-(Polar &v);
Polar operator-(const Polar &v) const;
Polar operator-=(const Polar &v);
/// @brief Add a polar vector to this vector
/// @param v The vector to add
/// @return The result of the addition
Polar operator+(Polar &v);
Polar operator+(const Polar &v) const;
Polar operator+=(const Polar &v);
/// @brief Scale the vector uniformly up
/// @param f The scaling factor
/// @return The scaled vector
/// @remark This operation will scale the distance of the vector. The angle
/// will be unaffected.
Polar operator*(float f) const;
friend Polar operator*(const Polar &v, float f);
friend Polar operator*(float f, const Polar &v);
Polar operator*=(float f);
/// @brief Scale the vector uniformly down
/// @param f The scaling factor
/// @return The scaled factor
/// @remark This operation will scale the distance of the vector. The angle
/// will be unaffected.
Polar operator/(const float &f);
friend Polar operator/(const Polar &v, float f);
friend Polar operator/(float f, const Polar &v);
Polar operator/=(float f);
/// @brief The distance between two vectors
/// @param v1 The first vector

61
Spherical.cpp
View File

@ -43,13 +43,55 @@ Spherical::Spherical(Vector3 v) {
}
const Spherical Spherical::zero = Spherical(0.0f, 0.0f, 0.0f);
const Spherical Spherical::forward = Spherical(1.0f, 0.0f, 0.0f);
const Spherical Spherical::back = Spherical(1.0f, 180.0f, 0.0f);
const Spherical Spherical::right = Spherical(1.0f, 90.0f, 0.0f);
const Spherical Spherical::left = Spherical(1.0f, -90.0f, 0.0f);
const Spherical Spherical::up = Spherical(1.0f, 0.0f, 90.0f);
const Spherical Spherical::down = Spherical(1.0f, 0.0f, -90.0f);
Spherical Spherical::operator-() {
bool Spherical::operator==(const Spherical &v) const {
return (this->distance == v.distance &&
this->horizontalAngle == v.horizontalAngle &&
this->verticalAngle == v.verticalAngle);
}
Spherical Spherical::Normalize(const Spherical &v) {
Spherical r = Spherical(1, v.horizontalAngle, v.verticalAngle);
return r;
}
Spherical Spherical::normalized() const {
Spherical r = Spherical(1, this->horizontalAngle, this->verticalAngle);
return r;
}
Spherical Spherical::operator-() const {
Spherical v = Spherical(this->distance, this->horizontalAngle + 180.0f,
this->verticalAngle + 180.0f);
return v;
}
Spherical Passer::operator*(const Spherical &v, float f) {
return Spherical(v.distance * f, v.horizontalAngle, v.verticalAngle);
}
Spherical Passer::operator*(float f, const Spherical &v) {
return Spherical(v.distance * f, v.horizontalAngle, v.verticalAngle);
}
Spherical Spherical::operator*=(float f) {
this->distance *= f;
return *this;
}
Spherical Passer::operator/(const Spherical &v, float f) {
return Spherical(v.distance / f, v.horizontalAngle, v.verticalAngle);
}
Spherical Passer::operator/(float f, const Spherical &v) {
return Spherical(v.distance / f, v.horizontalAngle, v.verticalAngle);
}
Spherical Spherical::operator/=(float f) {
this->distance /= f;
return *this;
}
// float Spherical::GetSwing() {
// // Not sure if this is correct
// return sqrtf(horizontalAngle * horizontalAngle +
@ -59,4 +101,19 @@ Spherical Spherical::operator-() {
// float Spherical::Distance(const Spherical &s1, const Spherical &s2) {
// float d = 0;
// return d;
// }
// }
Spherical Spherical::Rotate(const Spherical &v, Angle horizontalAngle,
Angle verticalAngle) {
Spherical r = Spherical(v.distance, v.horizontalAngle + horizontalAngle,
v.verticalAngle + verticalAngle);
return r;
}
Spherical Spherical::RotateHorizontal(const Spherical &v, Angle a) {
Spherical r = Spherical(v.distance, v.horizontalAngle + a, v.verticalAngle);
return r;
}
Spherical Spherical::RotateVertical(const Spherical &v, Angle a) {
Spherical r = Spherical(v.distance, v.horizontalAngle, v.verticalAngle + a);
return r;
}

68
Spherical.h
View File

@ -1,6 +1,6 @@
/// 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/.
// 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/.
#ifndef SPHERICAL_H
#define SPHERICAL_H
@ -13,8 +13,7 @@ namespace Passer {
struct Vector3;
/// @brief A spherical vector
/// @details
/// This is a vector in 3D space using a spherical coordinate system.
/// @details This is a vector in 3D space using a spherical coordinate system.
/// It consists of a distance and the polar and elevation angles from a
/// reference direction. The reference direction is typically thought of
/// as a forward direction.
@ -48,12 +47,64 @@ public:
/// @brief A spherical vector with zero degree angles and distance
const static Spherical zero;
/// @brief A normalized forward-oriented vector
const static Spherical forward;
/// @brief A normalized back-oriented vector
const static Spherical back;
/// @brief A normalized right-oriented vector
const static Spherical right;
/// @brief A normalized left-oriented vector
const static Spherical left;
/// @brief A normalized up-oriented vector
const static Spherical up;
/// @brief A normalized down-oriented vector
const static Spherical down;
/// @brief Equality test to another vector
/// @param v The vector to check against
/// @return true: if it is identical to the given vector
/// @note This uses float comparison to check equality which may have strange
/// effects. Equality on floats should be avoided.
bool operator==(const Spherical &v) const;
/// @brief The vector length
/// @param v The vector for which you need the length
/// @return The vector length;
inline static float Magnitude(const Spherical &v) { return v.distance; }
/// @brief The vector length
/// @return The vector length
inline float magnitude() const { return this->distance; }
/// @brief Convert the vector to a length of 1
/// @param v The vector to convert
/// @return The vector normalized to a length of 1
static Spherical Normalize(const Spherical &v);
/// @brief Convert the vector to a length of a
/// @return The vector normalized to a length of 1
Spherical normalized() const;
/// @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-();
Spherical operator-() const;
/// @brief Scale the vector uniformly up
/// @param f The scaling factor
/// @return The scaled vector
/// @remark This operation will scale the distance of the vector. The angle
/// will be unaffected.
friend Spherical operator*(const Spherical &v, float f);
friend Spherical operator*(float f, const Spherical &v);
Spherical operator*=(float f);
/// @brief Scale the vector uniformly down
/// @param f The scaling factor
/// @return The scaled factor
/// @remark This operation will scale the distance of the vector. The angle
/// will be unaffected.
friend Spherical operator/(const Spherical &v, float f);
friend Spherical operator/(float f, const Spherical &v);
Spherical operator/=(float f);
/// <summary>
/// The distance between two vectors
@ -62,6 +113,11 @@ public:
/// <param name="v2">The second vector</param>
/// <returns>The distance between the two vectors</returns>
// static float Distance(const Spherical &s1, const Spherical &s2);
static Spherical Rotate(const Spherical &v, Angle horizontalAngle,
Angle verticalAngle);
static Spherical RotateHorizontal(const Spherical &v, Angle angle);
static Spherical RotateVertical(const Spherical &v, Angle angle);
};
} // namespace Passer

12
Vector2.cpp
View File

@ -99,24 +99,24 @@ Vector2 Vector2::operator+=(const Vector2 &v) {
Vector2 Vector2::Scale(const Vector2 &v1, const Vector2 &v2) {
return Vector2(v1.x * v2.x, v1.y * v2.y);
}
Vector2 Passer::operator*(const Vector2 &v, const float f) {
Vector2 Passer::operator*(const Vector2 &v, float f) {
return Vector2(v.x * f, v.y * f);
}
Vector2 Passer::operator*(const float f, const Vector2 &v) {
Vector2 Passer::operator*(float f, const Vector2 &v) {
return Vector2(v.x * f, v.y * f);
}
Vector2 Vector2::operator*=(const float f) {
Vector2 Vector2::operator*=(float f) {
this->x *= f;
this->y *= f;
return *this;
}
Vector2 Passer::operator/(const Vector2 &v, const float f) {
Vector2 Passer::operator/(const Vector2 &v, float f) {
return Vector2(v.x / f, v.y / f);
}
Vector2 Passer::operator/(const float f, const Vector2 &v) {
Vector2 Passer::operator/(float f, const Vector2 &v) {
return Vector2(v.x / f, v.y / f);
}
Vector2 Vector2::operator/=(const float f) {
Vector2 Vector2::operator/=(float f) {
this->x /= f;
this->y /= f;
return *this;

22
Vector2.h
View File

@ -59,16 +59,14 @@ public:
const static Vector2 zero;
/// @brief A vector with one for all axis
const static Vector2 one;
/// @brief A normalized forward-oriented vector
const static Vector2 forward;
/// @brief A normalized back-oriented vector
const static Vector2 back;
/// @brief A normalized right-oriented vector
const static Vector2 right;
/// @brief A normalized left-oriented vector
const static Vector2 left;
/// @brief A normalized forward-oriented vector
/// @note This is equal to Vector2::up
const static Vector2 forward;
/// @brief A normalized back-oriented vector
/// @note This is equal to Vector2::down
const static Vector2 back;
/// @brief A normalized up-oriented vector
/// @note This is a convenience function which is equal to Vector2::forward
const static Vector2 up;
@ -139,16 +137,16 @@ public:
/// @return The scaled vector
/// @remark Each component of the vector will be multipled with the same
/// factor f.
friend Vector2 operator*(const Vector2 &v, const float f);
friend Vector2 operator*(const float f, const Vector2 &v);
Vector2 operator*=(const float f);
friend Vector2 operator*(const Vector2 &v, float f);
friend Vector2 operator*(float f, const Vector2 &v);
Vector2 operator*=(float f);
/// @brief Scale the vector uniformly down
/// @param f The scaling factor
/// @return The scaled vector
/// @remark Each componet of the vector will be divided by the same factor.
friend Vector2 operator/(const Vector2 &v, const float f);
friend Vector2 operator/(const float f, const Vector2 &v);
Vector2 operator/=(const float f);
friend Vector2 operator/(const Vector2 &v, float f);
friend Vector2 operator/(float f, const Vector2 &v);
Vector2 operator/=(float f);
/// @brief The dot product of two vectors
/// @param v1 The first vector

12
Vector3.cpp
View File

@ -118,25 +118,25 @@ Vector3 Vector3::operator+=(const Vector3 &v) {
Vector3 Vector3::Scale(const Vector3 &v1, const Vector3 &v2) {
return Vector3(v1.x * v2.x, v1.y * v2.y, v1.z * v2.z);
}
Vector3 Passer::operator*(const Vector3 &v, const float f) {
Vector3 Passer::operator*(const Vector3 &v, float f) {
return Vector3(v.x * f, v.y * f, v.z * f);
}
Vector3 Passer::operator*(const float f, const Vector3 &v) {
Vector3 Passer::operator*(float f, const Vector3 &v) {
return Vector3(v.x * f, v.y * f, v.z * f);
}
Vector3 Vector3::operator*=(const float f) {
Vector3 Vector3::operator*=(float f) {
this->x *= f;
this->y *= f;
this->z *= f;
return *this;
}
Vector3 Passer::operator/(const Vector3 &v, const float f) {
Vector3 Passer::operator/(const Vector3 &v, float f) {
return Vector3(v.x / f, v.y / f, v.z / f);
}
Vector3 Passer::operator/(const float f, const Vector3 &v) {
Vector3 Passer::operator/(float f, const Vector3 &v) {
return Vector3(v.x / f, v.y / f, v.z / f);
}
Vector3 Vector3::operator/=(const float f) {
Vector3 Vector3::operator/=(float f) {
this->x /= f;
this->y /= f;
this->z /= f;

20
Vector3.h
View File

@ -64,6 +64,10 @@ public:
const static Vector3 zero;
/// @brief A vector with one for all axis
const static Vector3 one;
/// @brief A normalized forward-oriented vector
const static Vector3 forward;
/// @brief A normalized back-oriented vector
const static Vector3 back;
/// @brief A normalized right-oriented vector
const static Vector3 right;
/// @brief A normalized left-oriented vector
@ -72,10 +76,6 @@ public:
const static Vector3 up;
/// @brief A normalized down-oriented vector
const static Vector3 down;
/// @brief A normalized forward-oriented vector
const static Vector3 forward;
/// @brief A normalized back-oriented vector
const static Vector3 back;
// Access functions which are intended to replace the use of XYZ
inline float Forward() const { return z; };
@ -145,16 +145,16 @@ public:
/// @return The scaled vector
/// @remark Each component of the vector will be multipled with the same
/// factor f.
friend Vector3 operator*(const Vector3 &v, const float f);
friend Vector3 operator*(const float f, const Vector3 &v);
Vector3 operator*=(const float f);
friend Vector3 operator*(const Vector3 &v, float f);
friend Vector3 operator*(float f, const Vector3 &v);
Vector3 operator*=(float f);
/// @brief Scale the vector uniformly down
/// @param f The scaling factor
/// @return The scaled vector
/// @remark Each componet of the vector will be divided by the same factor.
friend Vector3 operator/(const Vector3 &v, const float f);
friend Vector3 operator/(const float f, const Vector3 &v);
Vector3 operator/=(const float f);
friend Vector3 operator/(const Vector3 &v, float f);
friend Vector3 operator/(float f, const Vector3 &v);
Vector3 operator/=(float f);
/// @brief The distance between two vectors
/// @param v1 The first vector

4
test/Polar_test.cc
View File

@ -116,6 +116,10 @@ TEST(Polar, Addition) {
v2 = Polar::zero;
r = v1 + v2;
EXPECT_FLOAT_EQ(r.distance, v1.distance) << "Addition(0 0)";
r = v1;
r += v2;
EXPECT_FLOAT_EQ(r.distance, v1.distance) << "Addition(0 0)";
}
TEST(Polar, Scale_Multiply) {