108 lines
3.6 KiB
C++
108 lines
3.6 KiB
C++
// 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->swing = DirectionOf<T>(AngleOf<T>(), AngleOf<T>());
|
|
this->twist = AngleOf<T>();
|
|
}
|
|
|
|
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() const {
|
|
Quaternion q = Quaternion::Euler(this->swing.vertical.InDegrees(),
|
|
this->swing.horizontal.InDegrees(),
|
|
this->twist.InDegrees());
|
|
return q;
|
|
}
|
|
|
|
template <typename T>
|
|
SwingTwistOf<T> Passer::LinearAlgebra::SwingTwistOf<T>::FromQuaternion(
|
|
Quaternion q) {
|
|
Vector3 angles = Quaternion::ToAngles(q);
|
|
SwingTwistOf<T> r = SwingTwistOf<T>(AngleOf<T>::Degrees(angles.Up()),
|
|
AngleOf<T>::Degrees(angles.Right()),
|
|
AngleOf<T>::Degrees(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.direction.horizontal + this->swing.horizontal,
|
|
vector.direction.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->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.horizontal, -rotation.swing.vertical, -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>; |