Add Pose class

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<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(Quaternion<T> orientation, Vector3_SIMD<T> position)
+			: m_orientation(orientation), 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, 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;
+}