diff --git a/openVulkanoCpp/Math/Pose.hpp b/openVulkanoCpp/Math/Pose.hpp index db2b3ba..67e4d1a 100644 --- a/openVulkanoCpp/Math/Pose.hpp +++ b/openVulkanoCpp/Math/Pose.hpp @@ -1,8 +1,82 @@ -// -// Created by georg on 6/8/23. -// +/* + * 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 OPENVULKANOCPP_POSE_HPP -#define OPENVULKANOCPP_POSE_HPP +#pragma once -#endif //OPENVULKANOCPP_POSE_HPP +#include "Timestamp.hpp" +#include "Math.hpp" + +namespace openVulkanoCpp::Math +{ + template>> + class Pose + { + Quaternion m_orientation; + Vector3_SIMD m_position; + + public: + Pose(Quaternion orientation, Vector3_SIMD position) + : m_orientation(orientation), m_position(position) + {} + + [[nodiscard]] Quaternion& GetOrientation() const { return m_orientation; } + + [[nodiscard]] Vector3_SIMD GetPosition() const { return m_position; } + + [[nodiscard]] Pose Interpolate(const Pose& otherPose, T mixFactor) const + { + return Pose( + Utils::mix(m_orientation, otherPose.m_orientation, mixFactor), + Utils::mix(m_position, otherPose.m_position, mixFactor) + ); + } + + [[nodiscard]] Math::Matrix4 ToMatrix() const + { + Math::Matrix4 mat = Utils::toMat4(m_orientation); + mat[3] = Math::Vector4(m_position, 1); + return mat; + } + }; + + typedef Pose PoseF; + typedef Pose PoseD; + + template>> + class PoseWithTimestamp final : public Pose + { + Timestamp m_timestamp; + + public: + PoseWithTimestamp(Timestamp timestamp, Quaternion orientation, Vector3_SIMD position) + : m_timestamp(timestamp), Pose(orientation, position) + {} + + PoseWithTimestamp(Timestamp timestamp, Pose pose) + : m_timestamp(timestamp), Pose(pose) + {} + + [[nodiscard]] PoseWithTimestamp 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(targetTime, start->Interpolate(*end, mixFactor)); + } + + [[nodiscard]] Timestamp GetTimestamp() { return m_timestamp; } + }; + + typedef PoseWithTimestamp PoseWithTimestampF; + typedef PoseWithTimestamp PoseWithTimestampD; +}