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

Extend functionality

Browse Source
This commit is contained in:
Pascal Serrarens 2024-09-20 12:29:47 +02:00
parent 3363388a95
commit 11259a92a6
6 changed files with 210 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
Direction.cpp
View File

@ -41,6 +41,12 @@ const DirectionOf<T> DirectionOf<T>::left = DirectionOf<T>(-90.0f, 0.0f);
template <typename T>
const DirectionOf<T> DirectionOf<T>::right = DirectionOf<T>(90.0f, 0.0f);
template <typename T>
DirectionOf<T> Passer::LinearAlgebra::DirectionOf<T>::operator-() const {
DirectionOf<T> r = DirectionOf<T>(-this->horizontal, -this->vertical);
return r;
}
template <typename T>
DirectionOf<T> Passer::LinearAlgebra::DirectionOf<T>::operator+(
const DirectionOf<T>& v) const {

1
Direction.h
View File

@ -29,6 +29,7 @@ class DirectionOf {
const static DirectionOf left;
const static DirectionOf right;
DirectionOf<T> operator-() const;
DirectionOf<T> operator+(const DirectionOf<T>& v) const;
DirectionOf<T> operator+=(const DirectionOf<T>& v);

29
Spherical.cpp
View File

@ -70,10 +70,9 @@ SphericalOf<T> SphericalOf<T>::FromVector3(Vector3 v) {
template <typename T>
Vector3 SphericalOf<T>::ToVector3() const {
float verticalRad =
(90.0f - this->vertical.ToFloat()) * Passer::LinearAlgebra::Deg2Rad;
float horizontalRad =
this->horizontal.ToFloat() * Passer::LinearAlgebra::Deg2Rad;
float verticalRad = (pi / 2) - this->vertical.InRadians();
float horizontalRad = this->horizontal.InRadians();
float cosVertical = cosf(verticalRad);
float sinVertical = sinf(verticalRad);
float cosHorizontal = cosf(horizontalRad);
@ -217,8 +216,12 @@ const float epsilon = 1E-05f;
template <typename T>
float SphericalOf<T>::DistanceBetween(const SphericalOf<T>& v1,
const SphericalOf<T>& v2) {
SphericalOf<T> difference = v1 - v2;
return difference.distance;
// SphericalOf<T> difference = v1 - v2;
// return difference.distance;
Vector3 vec1 = v1.ToVector3();
Vector3 vec2 = v2.ToVector3();
float distance = Vector3::Distance(vec1, vec2);
return distance;
}
template <typename T>
@ -238,7 +241,19 @@ AngleOf<T> SphericalOf<T>::AngleBetween(const SphericalOf& v1,
// float cdot = Float::Clamp(fraction, -1.0, 1.0);
// float r = ((float)acos(cdot)) * Rad2Deg;
Angle r = Vector3::Angle(v1_3, v2_3);
return AngleOf<T>(r.ToFloat());
return AngleOf<T>::Degrees(r.InDegrees());
}
template <typename T>
AngleOf<T> Passer::LinearAlgebra::SphericalOf<T>::SignedAngleBetween(
const SphericalOf<T>& v1,
const SphericalOf<T>& v2,
const SphericalOf<T>& axis) {
Vector3 v1_vector = v1.ToVector3();
Vector3 v2_vector = v2.ToVector3();
Vector3 axis_vector = axis.ToVector3();
Angle r = Vector3::SignedAngle(v1_vector, v2_vector, axis_vector);
return AngleOf<T>::Degrees(r.InDegrees());
}
template <typename T>

3
Spherical.h
View File

@ -109,6 +109,9 @@ class SphericalOf {
const SphericalOf<T>& v2);
static AngleOf<T> AngleBetween(const SphericalOf<T>& v1,
const SphericalOf<T>& v2);
static AngleOf<T> SignedAngleBetween(const SphericalOf<T>& v1,
const SphericalOf<T>& v2,
const SphericalOf<T>& axis);
static SphericalOf<T> Rotate(const SphericalOf& v,
AngleOf<T> horizontalAngle,

109
SwingTwist.cpp Normal file
View File

@ -0,0 +1,109 @@
// 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 "SwingTwist.h"
template <typename T>
SwingTwistOf<T>::SwingTwistOf() {
// this->horizontal = AngleOf<T>(0);
// this->vertical = AngleOf<T>(0);
this->swing = DirectionOf<T>(0, 0);
this->twist = AngleOf<T>(0);
}
template <typename T>
Passer::LinearAlgebra::SwingTwistOf<T>::SwingTwistOf(DirectionOf<T> swing,
AngleOf<T> twist) {
this->swing = swing;
this->twist = twist;
}
template <typename T>
SwingTwistOf<T>::SwingTwistOf(AngleOf<T> horizontal,
AngleOf<T> vertical,
AngleOf<T> twist) {
// this->horizontal = horizontal;
// this->vertical = vertical;
this->swing = DirectionOf<T>(horizontal, vertical);
this->twist = twist;
}
template <typename T>
SwingTwistOf<T> SwingTwistOf<T>::Degrees(float horizontal,
float vertical,
float twist) {
DirectionOf<T> swing = DirectionOf<T>(AngleOf<T>::Degrees(horizontal),
AngleOf<T>::Degrees(vertical));
AngleOf<T> twistAngle = AngleOf<T>::Degrees(twist);
SwingTwistOf<T> orientation = SwingTwistOf(swing, twistAngle);
return orientation;
}
template <typename T>
Quaternion SwingTwistOf<T>::ToQuaternion() {
Quaternion q = Quaternion::Euler(this->swing.vertical.ToFloat(),
this->swing.horizontal.ToFloat(),
this->twist.ToFloat());
return q;
}
template <typename T>
SwingTwistOf<T> Passer::LinearAlgebra::SwingTwistOf<T>::FromQuaternion(
Quaternion q) {
Vector3 angles = Quaternion::ToAngles(q);
SwingTwistOf<T> r =
SwingTwistOf<T>(angles.Up(), angles.Right(), angles.Forward());
return r;
}
template <typename T>
const SwingTwistOf<T> SwingTwistOf<T>::identity = SwingTwistOf();
template <typename T>
SphericalOf<T> SwingTwistOf<T>::operator*(const SphericalOf<T>& vector) const {
SphericalOf<T> v = SphericalOf<T>(vector.distance,
vector.horizontal + this->swing.horizontal,
vector.vertical + this->swing.vertical);
return v;
}
template <typename T>
SwingTwistOf<T> SwingTwistOf<T>::operator*(
const SwingTwistOf<T>& rotation) const {
SwingTwistOf<T> r =
SwingTwistOf(this->swing.horizontal + rotation.swing.horizontal,
this->swing.vertical + rotation.swing.vertical,
this->twist + rotation.twist);
return r;
}
template <typename T>
SwingTwistOf<T> SwingTwistOf<T>::operator*=(const SwingTwistOf<T>& rotation) {
// this->swing.horizontal += rotation.swing.horizontal;
// this->swing.vertical += rotation.swing.vertical;
this->swing += rotation.swing;
this->twist += rotation.twist;
return *this;
}
template <typename T>
SwingTwistOf<T> Passer::LinearAlgebra::SwingTwistOf<T>::Inverse(
SwingTwistOf<T> rotation) {
SwingTwistOf<T> r = SwingTwistOf<T>(-rotation.swing, -rotation.twist);
return r;
}
template <typename T>
SwingTwistOf<T> Passer::LinearAlgebra::SwingTwistOf<T>::AngleAxis(
float angle,
const SphericalOf<T>& axis) {
Vector3 axis_vector = axis.ToVector3();
Quaternion q = Quaternion::AngleAxis(angle, axis_vector);
SwingTwistOf<T> r = SwingTwistOf<T>::FromQuaternion(q);
return r;
}
template class SwingTwistOf<float>;
template class SwingTwistOf<signed short>;

69
SwingTwist.h Normal file
View File

@ -0,0 +1,69 @@
// 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 SWINGTWIST_H
#define SWINGTWIST_H
#include "RoboidControlCore/LinearAlgebra/Angle.h"
#include "RoboidControlCore/LinearAlgebra/Direction.h"
#include "RoboidControlCore/LinearAlgebra/Quaternion.h"
#include "RoboidControlCore/LinearAlgebra/Spherical.h"
// #include "RoboidControlCore/LinearAlgebra/Spherical16.h"
namespace Passer {
namespace LinearAlgebra {
template <typename T>
class SwingTwistOf {
public:
DirectionOf<T> swing;
AngleOf<T> twist;
SwingTwistOf<T>();
SwingTwistOf<T>(DirectionOf<T> swing, AngleOf<T> twist);
SwingTwistOf<T>(AngleOf<T> horizontal, AngleOf<T> vertical, AngleOf<T> twist);
static SwingTwistOf<T> Degrees(float horizontal,
float vertical = 0,
float twist = 0);
Quaternion ToQuaternion();
static SwingTwistOf<T> FromQuaternion(Quaternion q);
const static SwingTwistOf<T> identity;
/// <summary>
/// Rotate a vector using this rotation
/// </summary>
/// <param name="vector">The vector to rotate</param>
/// <returns>The rotated vector</returns>
SphericalOf<T> operator*(const SphericalOf<T>& vector) const;
/// <summary>
/// Multiply this rotation with another rotation
/// </summary>
/// <param name="rotation">The swing/twist rotation to multiply with</param>
/// <returns>The resulting swing/twist rotation</returns>
/// The result will be this rotation rotated according to
/// the give rotation.
SwingTwistOf<T> operator*(const SwingTwistOf<T>& rotation) const;
SwingTwistOf<T> operator*=(const SwingTwistOf<T>& rotation);
static SwingTwistOf<T> Inverse(SwingTwistOf<T> rotation);
/// <summary>
/// Convert an angle/axis representation to a swingt
/// </summary>
/// <param name="angle">The angle</param>
/// <param name="axis">The axis</param>
/// <returns>The resulting quaternion</returns>
static SwingTwistOf<T> AngleAxis(float angle, const SphericalOf<T>& axis);
};
using SwingTwistSingle = SwingTwistOf<float>;
using SwingTwist16 = SwingTwistOf<signed short>;
} // namespace LinearAlgebra
} // namespace Passer
using namespace Passer::LinearAlgebra;
#endif