126 lines
3.9 KiB
C++
126 lines
3.9 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/.
|
|
*/
|
|
|
|
#pragma once
|
|
|
|
#include "Timestamp.hpp"
|
|
#include "Math.hpp"
|
|
|
|
namespace OpenVulkano::Math
|
|
{
|
|
template<typename T, typename = std::enable_if<std::is_floating_point_v<T>>>
|
|
class Pose
|
|
{
|
|
Quaternion<T> m_orientation;
|
|
Vector3_SIMD<T> m_position;
|
|
|
|
public:
|
|
Pose() : m_orientation(1,0,0,0), m_position(0) {}
|
|
|
|
Pose(const Quaternion<T>& orientation, const Vector3_SIMD<T>& position)
|
|
: m_orientation(orientation), m_position(position)
|
|
{}
|
|
|
|
Pose(const Math::Vector3_SIMD<T>& eulerAngle, const Vector3f_SIMD& position)
|
|
: m_orientation(Math::Utils::qua(eulerAngle)), m_position(position)
|
|
{}
|
|
|
|
Pose(const Math::Matrix4<T> &matrix)
|
|
{
|
|
Math::Vector3_SIMD<T> scale;
|
|
Math::Quaternion<T> rotation;
|
|
Math::Vector3_SIMD<T> translation;
|
|
Math::Vector3_SIMD<T> skew;
|
|
Math::Vector4_SIMD<T> perspective;
|
|
|
|
bool decomposed = Math::Utils::decompose(matrix, scale, rotation, translation, skew, perspective);
|
|
if(decomposed)
|
|
{
|
|
m_orientation = rotation;
|
|
m_position = translation;
|
|
}
|
|
}
|
|
|
|
bool operator==(const Math::Pose<T> &otherPose) const
|
|
{
|
|
return (GetOrientation() == otherPose.GetOrientation()) && (GetPosition() == otherPose.GetPosition());
|
|
}
|
|
|
|
[[nodiscard]] const Quaternion<T>& GetOrientation() const { return m_orientation; }
|
|
|
|
[[nodiscard]] Quaternion<T>& GetOrientation() { return m_orientation; }
|
|
|
|
[[nodiscard]] Math::Vector3f GetOrientationAsEuler() const { return Math::Utils::eulerAngles(m_orientation); }
|
|
|
|
void SetOrientation(const Math::Quaternion<T>& orientation) { m_orientation = orientation; }
|
|
|
|
void SetOrientation(const Math::Vector3_SIMD<T>& eulerAngle) { m_orientation = Math::Utils::qua(eulerAngle); }
|
|
|
|
[[nodiscard]] const Vector3_SIMD<T>& GetPosition() const { return m_position; }
|
|
|
|
[[nodiscard]] Vector3_SIMD<T>& GetPosition() { return m_position; }
|
|
|
|
void SetPosition(const Math::Vector3_SIMD<T>& pos) { m_position = pos; }
|
|
|
|
[[nodiscard]] Pose<T> Interpolate(const Pose<T>& otherPose, T mixFactor) const
|
|
{
|
|
return Pose<T>(
|
|
Utils::slerp(m_orientation, otherPose.m_orientation, mixFactor),
|
|
Utils::mix(m_position, otherPose.m_position, mixFactor)
|
|
);
|
|
}
|
|
|
|
[[nodiscard]] Math::Matrix4<T> ToMatrix() const
|
|
{
|
|
Math::Matrix4<T> mat = Utils::toMat4(m_orientation);
|
|
mat[3] = Math::Vector4<T>(m_position, 1);
|
|
return mat;
|
|
}
|
|
};
|
|
|
|
typedef Pose<float> PoseF;
|
|
typedef Pose<double> PoseD;
|
|
|
|
template<typename T, typename = std::enable_if<std::is_floating_point_v<T>>>
|
|
class PoseWithTimestamp final : public Pose<T>
|
|
{
|
|
Timestamp m_timestamp;
|
|
|
|
public:
|
|
PoseWithTimestamp(Timestamp timestamp, Quaternion<T> orientation, Vector3_SIMD<T> position)
|
|
: m_timestamp(timestamp), Pose<T>(orientation, position)
|
|
{}
|
|
|
|
PoseWithTimestamp(Timestamp timestamp, Math::Vector3_SIMD<T> eulerAngle, Vector3_SIMD<T> position)
|
|
: m_timestamp(timestamp), Pose<T>(eulerAngle, position)
|
|
{}
|
|
|
|
PoseWithTimestamp(Timestamp timestamp, Pose<T> pose)
|
|
: m_timestamp(timestamp), Pose<T>(pose)
|
|
{}
|
|
|
|
[[nodiscard]] PoseWithTimestamp<T> Interpolate(const PoseWithTimestamp& otherPose, Timestamp targetTime) const
|
|
{
|
|
PoseWithTimestamp* start = *this;
|
|
PoseWithTimestamp* end = &otherPose;
|
|
if (start->m_timestamp == end->m_timestamp) return *this; // Nothing to interpolate
|
|
if (start->m_timestamp > end->m_timestamp)
|
|
{
|
|
std::swap(start, end);
|
|
}
|
|
if (targetTime <= start->m_timestamp) return *start;
|
|
if (targetTime >= end->m_timestamp) return *end;
|
|
T mixFactor = (targetTime - start->m_timestamp) / (end->m_timestamp - start->m_timestamp);
|
|
return PoseWithTimestamp<T>(targetTime, start->Interpolate(*end, mixFactor));
|
|
}
|
|
|
|
[[nodiscard]] Timestamp GetTimestamp() { return m_timestamp; }
|
|
};
|
|
|
|
typedef PoseWithTimestamp<float> PoseWithTimestampF;
|
|
typedef PoseWithTimestamp<double> PoseWithTimestampD;
|
|
}
|