Implementing basic scene tree

src/Engine/Math/Vector.h

@@ -2,10 +2,11 @@
 #include <glm/vec2.hpp>
 #include <glm/vec3.hpp>
 #include <glm/vec4.hpp>
+#include <glm/gtc/quaternion.hpp>
 namespace Seele
 {
 typedef glm::vec2 Vector2;
-typedef glm::vec3 Vector3;
+typedef glm::vec3 Vector;
 typedef glm::vec4 Vector4;
 
 typedef glm::uvec2 UVector2;
@@ -15,4 +16,101 @@ typedef glm::uvec4 UVector4;
 typedef glm::ivec2 IVector2;
 typedef glm::ivec3 IVector3;
 typedef glm::ivec4 IVector4;
+
+typedef glm::quat Quaternion;
+
+static inline float square(float x)
+{
+    return x * x;
+}
+
+static inline Vector unrotateVector(Quaternion quaternion, Vector v)
+{
+	const Vector q(-quaternion.x, -quaternion.y, -quaternion.z);
+	const Vector t = 2.f * glm::cross(q, v);
+	const Vector result = v + (quaternion.w * t) + glm::cross(q, t);
+	return result;
+}
+
+static inline bool equalsQuaternion(const Quaternion& right, const Quaternion& left, float tolerance)
+{
+	return (abs(right.x - left.x) <= tolerance && abs(right.y - left.y) <= tolerance && abs(right.z - left.z) <= tolerance && abs(right.w - left.w) <= tolerance)
+		|| (abs(right.x + left.x) <= tolerance && abs(right.y + left.y) <= tolerance && abs(right.z + left.z) <= tolerance && abs(right.w + left.w) <= tolerance);
+}
+
+static inline float clampRotatorAxis(float angle)
+{
+	angle = fmod(angle, 360.f);
+	if (angle < 0.f)
+	{
+		angle += 360.f;
+	}
+	return angle;
+}
+static inline float normalizeRotatorAxis(float angle)
+{
+	angle = clampRotatorAxis(angle);
+
+	if (angle > 180.f)
+	{
+		angle -= 360.f;
+	}
+	return angle;
+}
+
+static inline Quaternion toQuaternion(const Vector& other)
+{
+	Quaternion result;
+	const float DEG_TO_RAD = glm::pi<float>() / (180.f);
+	const float RADS_DIVIDED_BY_2 = DEG_TO_RAD / 2.f;
+
+	const float PitchNoWinding = fmod(other.x, 360.0f);
+	const float YawNoWinding = fmod(other.y, 360.0f);
+	const float RollNoWinding = fmod(other.z, 360.0f);
+
+	const float SP = sin(PitchNoWinding * RADS_DIVIDED_BY_2);
+	const float SY = sin(YawNoWinding * RADS_DIVIDED_BY_2);
+	const float SR = sin(RollNoWinding * RADS_DIVIDED_BY_2);
+	
+	const float CP = cos(PitchNoWinding * RADS_DIVIDED_BY_2);
+	const float CY = cos(YawNoWinding * RADS_DIVIDED_BY_2);
+	const float CR = cos(RollNoWinding * RADS_DIVIDED_BY_2);
+
+	result.x = CR * SP * SY - SR * CP * CY;
+	result.y = -CR * SP * CY - SR * CP * SY;
+	result.z = CR * CP * SY - SR * SP * CY;
+	result.w = CR * CP * CY + SR * SP * SY;
+	return result;
+}
+
+static inline Vector toRotator(const Quaternion& other)
+{
+	const float singularityTest = other.z * other.x - other.w * other.y;
+	const float yawY = 2.f * (other.w * other.z + other.x * other.y);
+	const float yawX = (1.f - 2.f * (square(other.y) + square(other.z)));
+	
+	const float SINGULARITY_THRESHOLD = 0.4999995f;
+	const float RAD_TO_DEG = (180.f) / glm::pi<float>();
+	Vector rotatorFromQuat;
+
+	if (singularityTest < -SINGULARITY_THRESHOLD)
+	{
+		rotatorFromQuat.x = -90.f;
+		rotatorFromQuat.y = atan2(yawY, yawX) * RAD_TO_DEG;
+		rotatorFromQuat.z = normalizeRotatorAxis(-rotatorFromQuat.y - (2.f * atan2(other.x, other.w) * RAD_TO_DEG));
+	}
+	else if (singularityTest > SINGULARITY_THRESHOLD)
+	{
+		rotatorFromQuat.x = 90.f;
+		rotatorFromQuat.y = atan2(yawY, yawX) * RAD_TO_DEG;
+		rotatorFromQuat.z = normalizeRotatorAxis(rotatorFromQuat.y - (2.f * atan2(other.x, other.w) * RAD_TO_DEG));
+	}
+	else
+	{
+		rotatorFromQuat.x = asin(2.f * (singularityTest)) * RAD_TO_DEG;
+		rotatorFromQuat.y = atan2(yawY, yawX) * RAD_TO_DEG;
+		rotatorFromQuat.z = atan2(-2.f * (other.w * other.x + other.y * other.z), (1.f - 2.f * (square(other.x) + square(other.y)))) * RAD_TO_DEG;
+	}
+	return rotatorFromQuat;
+}
 } // namespace Seele