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

Normalize Spherical at creation

Browse Source
This commit is contained in:
Pascal Serrarens 2024-12-18 16:54:33 +01:00
parent 800eb75e28
commit 28ee2254b8
1 changed files with 34 additions and 61 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

95
Spherical.cpp
View File

@ -5,55 +5,34 @@
#include <math.h> #include <math.h>
template <typename T> template <typename T> SphericalOf<T>::SphericalOf() {
SphericalOf<T>::SphericalOf() {
this->distance = 0.0f; this->distance = 0.0f;
this->direction = DirectionOf<T>(); this->direction = DirectionOf<T>();
// this->horizontal = AngleOf<T>();
// this->vertical = AngleOf<T>();
} }
// template <>
// SphericalOf<signed short>::SphericalOf() {
// this->distance = 0.0f;
// this->horizontal = AngleOf<signed short>(0);
// this->vertical = AngleOf<signed short>(0);
// }
template <typename T> template <typename T>
SphericalOf<T>::SphericalOf(float distance, SphericalOf<T>::SphericalOf(float distance, AngleOf<T> horizontal,
AngleOf<T> horizontal,
AngleOf<T> vertical) { AngleOf<T> vertical) {
this->distance = distance; if (distance < 0) {
this->direction = DirectionOf<T>(horizontal, vertical); this->distance = distance;
// this->horizontal = horizontal; this->direction = DirectionOf<T>(horizontal, vertical);
// this->vertical = vertical; } else {
this->distance = -distance;
this->direction = -DirectionOf<T>(horizontal, vertical);
}
} }
template <typename T> template <typename T>
SphericalOf<T>::SphericalOf(float distance, DirectionOf<T> direction) { SphericalOf<T>::SphericalOf(float distance, DirectionOf<T> direction) {
this->distance = distance; if (distance < 0) {
this->direction = direction; this->distance = -distance;
this->direction = -direction;
} else {
this->distance = distance;
this->direction = direction;
}
} }
// template <>
// SphericalOf<float>::SphericalOf(float distance,
// AngleOf<float> horizontal,
// AngleOf<float> vertical) {
// this->distance = distance;
// this->horizontal = horizontal;
// this->vertical = vertical;
// }
// template <>
// SphericalOf<signed short>::SphericalOf(float distance,
// AngleOf<signed short> horizontal,
// AngleOf<signed short> vertical) {
// this->distance = distance;
// this->horizontal = horizontal;
// this->vertical = vertical;
// }
template <typename T> template <typename T>
SphericalOf<T> SphericalOf<T>::FromPolar(PolarOf<T> polar) { SphericalOf<T> SphericalOf<T>::FromPolar(PolarOf<T> polar) {
AngleOf<T> horizontal = polar.angle; AngleOf<T> horizontal = polar.angle;
@ -62,8 +41,7 @@ SphericalOf<T> SphericalOf<T>::FromPolar(PolarOf<T> polar) {
return r; return r;
} }
template <typename T> template <typename T> SphericalOf<T> SphericalOf<T>::FromVector3(Vector3 v) {
SphericalOf<T> SphericalOf<T>::FromVector3(Vector3 v) {
float distance = v.magnitude(); float distance = v.magnitude();
if (distance == 0.0f) { if (distance == 0.0f) {
return SphericalOf(distance, AngleOf<T>(), AngleOf<T>()); return SphericalOf(distance, AngleOf<T>(), AngleOf<T>());
@ -76,8 +54,7 @@ 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->direction.vertical.InRadians(); float verticalRad = (pi / 2) - this->direction.vertical.InRadians();
float horizontalRad = this->direction.horizontal.InRadians(); float horizontalRad = this->direction.horizontal.InRadians();
@ -122,8 +99,7 @@ SphericalOf<T> SphericalOf<T>::WithDistance(float distance) {
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>( SphericalOf<T> v = SphericalOf<T>(
this->distance, this->direction.horizontal + AngleOf<T>::Degrees(180), this->distance, this->direction.horizontal + AngleOf<T>::Degrees(180),
this->direction.vertical + AngleOf<T>::Degrees(180)); this->direction.vertical + AngleOf<T>::Degrees(180));
@ -131,7 +107,7 @@ SphericalOf<T> SphericalOf<T>::operator-() const {
} }
template <typename T> template <typename T>
SphericalOf<T> SphericalOf<T>::operator-(const SphericalOf<T>& s2) const { SphericalOf<T> SphericalOf<T>::operator-(const SphericalOf<T> &s2) const {
// let's do it the easy way... // let's do it the easy way...
Vector3 v1 = this->ToVector3(); Vector3 v1 = this->ToVector3();
Vector3 v2 = s2.ToVector3(); Vector3 v2 = s2.ToVector3();
@ -140,13 +116,13 @@ SphericalOf<T> SphericalOf<T>::operator-(const SphericalOf<T>& s2) const {
return r; return r;
} }
template <typename T> template <typename T>
SphericalOf<T> SphericalOf<T>::operator-=(const SphericalOf<T>& v) { SphericalOf<T> SphericalOf<T>::operator-=(const SphericalOf<T> &v) {
*this = *this - v; *this = *this - v;
return *this; return *this;
} }
template <typename T> template <typename T>
SphericalOf<T> SphericalOf<T>::operator+(const SphericalOf<T>& s2) const { SphericalOf<T> SphericalOf<T>::operator+(const SphericalOf<T> &s2) const {
// let's do it the easy way... // let's do it the easy way...
Vector3 v1 = this->ToVector3(); Vector3 v1 = this->ToVector3();
Vector3 v2 = s2.ToVector3(); Vector3 v2 = s2.ToVector3();
@ -201,19 +177,17 @@ SphericalOf<T> SphericalOf<T>::operator+(const SphericalOf<T>& s2) const {
*/ */
} }
template <typename T> template <typename T>
SphericalOf<T> SphericalOf<T>::operator+=(const SphericalOf<T>& v) { SphericalOf<T> SphericalOf<T>::operator+=(const SphericalOf<T> &v) {
*this = *this + v; *this = *this + v;
return *this; return *this;
} }
template <typename T> template <typename T> SphericalOf<T> SphericalOf<T>::operator*=(float f) {
SphericalOf<T> SphericalOf<T>::operator*=(float f) {
this->distance *= f; this->distance *= f;
return *this; return *this;
} }
template <typename T> template <typename T> SphericalOf<T> SphericalOf<T>::operator/=(float f) {
SphericalOf<T> SphericalOf<T>::operator/=(float f) {
this->distance /= f; this->distance /= f;
return *this; return *this;
} }
@ -224,8 +198,8 @@ SphericalOf<T> SphericalOf<T>::operator/=(float f) {
const float epsilon = 1E-05f; const float epsilon = 1E-05f;
template <typename T> template <typename T>
float SphericalOf<T>::DistanceBetween(const SphericalOf<T>& v1, float SphericalOf<T>::DistanceBetween(const SphericalOf<T> &v1,
const SphericalOf<T>& v2) { const SphericalOf<T> &v2) {
// SphericalOf<T> difference = v1 - v2; // SphericalOf<T> difference = v1 - v2;
// return difference.distance; // return difference.distance;
Vector3 vec1 = v1.ToVector3(); Vector3 vec1 = v1.ToVector3();
@ -235,8 +209,8 @@ float SphericalOf<T>::DistanceBetween(const SphericalOf<T>& v1,
} }
template <typename T> template <typename T>
AngleOf<T> SphericalOf<T>::AngleBetween(const SphericalOf& v1, AngleOf<T> SphericalOf<T>::AngleBetween(const SphericalOf &v1,
const SphericalOf& v2) { const SphericalOf &v2) {
// float denominator = v1.distance * v2.distance; // float denominator = v1.distance * v2.distance;
// if (denominator < epsilon) // if (denominator < epsilon)
// return 0.0f; // return 0.0f;
@ -256,9 +230,8 @@ AngleOf<T> SphericalOf<T>::AngleBetween(const SphericalOf& v1,
template <typename T> template <typename T>
AngleOf<T> Passer::LinearAlgebra::SphericalOf<T>::SignedAngleBetween( AngleOf<T> Passer::LinearAlgebra::SphericalOf<T>::SignedAngleBetween(
const SphericalOf<T>& v1, const SphericalOf<T> &v1, const SphericalOf<T> &v2,
const SphericalOf<T>& v2, const SphericalOf<T> &axis) {
const SphericalOf<T>& axis) {
Vector3 v1_vector = v1.ToVector3(); Vector3 v1_vector = v1.ToVector3();
Vector3 v2_vector = v2.ToVector3(); Vector3 v2_vector = v2.ToVector3();
Vector3 axis_vector = axis.ToVector3(); Vector3 axis_vector = axis.ToVector3();
@ -267,7 +240,7 @@ AngleOf<T> Passer::LinearAlgebra::SphericalOf<T>::SignedAngleBetween(
} }
template <typename T> 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<T> r =
@ -276,14 +249,14 @@ SphericalOf<T> SphericalOf<T>::Rotate(const SphericalOf<T>& v,
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<T> r =
SphericalOf(v.distance, v.direction.horizontal + a, v.direction.vertical); 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<T> r =
SphericalOf(v.distance, v.direction.horizontal, v.direction.vertical + a); SphericalOf(v.distance, v.direction.horizontal, v.direction.vertical + a);