91 lines
2.8 KiB
C++
91 lines
2.8 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 openVulkanoCpp::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(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)
|
|
{}
|
|
|
|
[[nodiscard]] Quaternion<T>& GetOrientation() const { return m_orientation; }
|
|
|
|
[[nodiscard]] Vector3_SIMD<T> GetPosition() const { return m_position; }
|
|
|
|
[[nodiscard]] Pose<T> Interpolate(const Pose<T>& otherPose, T mixFactor) const
|
|
{
|
|
return Pose<T>(
|
|
Utils::mix(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;
|
|
}
|