From fc6d2184fab32a2af4ccb1a41c2af4283cba47a9 Mon Sep 17 00:00:00 2001
From: Daniel Karbach <daniel.karbach@localhorst.tv>
Date: Fri, 7 Aug 2015 13:20:03 +0200
Subject: [PATCH] correct usage of quaternions :P

---
 src/ai/Spawner.cpp        |  2 +-
 src/app/FPSController.cpp |  2 +-
 src/world/Entity.cpp      | 40 +++++++++++++++++++++++++++------------
 src/world/Entity.hpp      | 17 ++++++++++-------
 4 files changed, 40 insertions(+), 21 deletions(-)

diff --git a/src/ai/Spawner.cpp b/src/ai/Spawner.cpp
index 8a3e75e..2340067 100644
--- a/src/ai/Spawner.cpp
+++ b/src/ai/Spawner.cpp
@@ -106,7 +106,7 @@ void Spawner::Spawn(const glm::tvec3<int> &chunk, const glm::vec3 &pos) {
 	e.Bounds({ { -0.5f, -0.5f, -0.5f }, { 0.5f, 0.5f, 0.5f } });
 	e.WorldCollidable(true);
 	e.SetShape(world.BlockTypes()[1].shape, color);
-	e.AngularVelocity(glm::quat(glm::vec3{ 0.00001f, 0.000006f, 0.000013f }));
+	e.AngularVelocity(rot);
 	Controller *ctrl;
 	if (rand() % 2) {
 		ctrl = new RandomWalk(e);
diff --git a/src/app/FPSController.cpp b/src/app/FPSController.cpp
index 9c7820c..48e39dc 100644
--- a/src/app/FPSController.cpp
+++ b/src/app/FPSController.cpp
@@ -42,7 +42,7 @@ void FPSController::RotateYaw(float delta) noexcept {
 
 
 void FPSController::Update(int dt) noexcept {
-	entity.Rotation(glm::eulerAngleYX(yaw, pitch));
+	entity.Rotation(glm::quat(glm::vec3(pitch, yaw, 0.0f)));
 	entity.Velocity(glm::rotateY(velocity, yaw));
 }
 
diff --git a/src/world/Entity.cpp b/src/world/Entity.cpp
index 49ad34b..4a34a11 100644
--- a/src/world/Entity.cpp
+++ b/src/world/Entity.cpp
@@ -3,6 +3,7 @@
 #include "../model/Shape.hpp"
 
 #include <cmath>
+#include <glm/gtx/quaternion.hpp>
 #include <glm/gtx/transform.hpp>
 
 namespace {
@@ -21,8 +22,8 @@ Entity::Entity() noexcept
 , velocity(0, 0, 0)
 , position(0, 0, 0)
 , chunk(0, 0, 0)
-, angular_velocity(1.0f, 0.0f, 0.0f, 0.0f)
-, rotation(1.0f)
+, angular_velocity(0.0f)
+, rotation(1.0f, 0.0f, 0.0f, 0.0f)
 , world_collision(false)
 , remove(false) {
 
@@ -83,21 +84,25 @@ void Entity::Move(const glm::vec3 &delta) noexcept {
 	Position(position + delta);
 }
 
-void Entity::AngularVelocity(const glm::quat &v) noexcept {
+void Entity::AngularVelocity(const glm::vec3 &v) noexcept {
 	angular_velocity = v;
 }
 
-void Entity::Rotation(const glm::mat4 &rot) noexcept {
+void Entity::Rotation(const glm::quat &rot) noexcept {
 	rotation = rot;
 }
 
 void Entity::Rotate(const glm::quat &delta) noexcept {
-	Rotation(rotation * glm::mat4_cast(delta));
+	Rotation(delta * Rotation());
 }
 
 glm::mat4 Entity::Transform(const Chunk::Pos &chunk_offset) const noexcept {
-	const glm::vec3 chunk_pos = (chunk - chunk_offset) * Chunk::Extent();
-	return glm::translate(position + chunk_pos) * rotation;
+	const glm::vec3 translation = glm::vec3((chunk - chunk_offset) * Chunk::Extent()) + position;
+	glm::mat4 transform(toMat4(Rotation()));
+	transform[3].x = translation.x;
+	transform[3].y = translation.y;
+	transform[3].z = translation.z;
+	return transform;
 }
 
 Ray Entity::Aim(const Chunk::Pos &chunk_offset) const noexcept {
@@ -109,14 +114,25 @@ Ray Entity::Aim(const Chunk::Pos &chunk_offset) const noexcept {
 	return Ray{ glm::vec3(from), glm::normalize(glm::vec3(to - from)) };
 }
 
-void Entity::Update(int dt) noexcept {
-	Move(velocity * float(dt));
-	Rotate(angular_velocity * float(dt));
+namespace {
+
+glm::quat delta_rot(const glm::vec3 &av, float dt) {
+	glm::vec3 half(av * dt * 0.5f);
+	float mag = length(half);
+	if (mag > 0.0f) {
+		float smag = std::sin(mag) / mag;
+		return glm::quat(std::cos(mag), half * smag);
+	} else {
+		return glm::quat(1.0f, 0.0f, 0.0f, 0.0f);
+	}
 }
 
+}
 
-void Entity::Draw() noexcept {
-	model.Draw();
+void Entity::Update(int dt) noexcept {
+	float fdt = float(dt);
+	Move(velocity * fdt);
+	Rotate(delta_rot(angular_velocity, fdt));
 }
 
 }
diff --git a/src/world/Entity.hpp b/src/world/Entity.hpp
index 25818c6..078daa6 100644
--- a/src/world/Entity.hpp
+++ b/src/world/Entity.hpp
@@ -51,11 +51,12 @@ public:
 		return glm::vec3((chunk - other.chunk) * Chunk::Extent()) + position - other.position;
 	}
 
-	const glm::quat &AngularVelocity() const noexcept { return angular_velocity; }
-	void AngularVelocity(const glm::quat &) noexcept;
+	/// direction is rotation axis, magnitude is speed in rad/ms
+	const glm::vec3 &AngularVelocity() const noexcept { return angular_velocity; }
+	void AngularVelocity(const glm::vec3 &) noexcept;
 
-	const glm::mat4 &Rotation() const noexcept { return rotation; }
-	void Rotation(const glm::mat4 &) noexcept;
+	const glm::quat &Rotation() const noexcept { return rotation; }
+	void Rotation(const glm::quat &) noexcept;
 	void Rotate(const glm::quat &delta) noexcept;
 
 	glm::mat4 Transform(const Chunk::Pos &chunk_offset) const noexcept;
@@ -66,7 +67,9 @@ public:
 
 	void Update(int dt) noexcept;
 
-	void Draw() noexcept;
+	void Draw() noexcept {
+		model.Draw();
+	}
 
 private:
 	const Shape *shape;
@@ -80,8 +83,8 @@ private:
 	Block::Pos position;
 	Chunk::Pos chunk;
 
-	glm::quat angular_velocity;
-	glm::mat4 rotation;
+	glm::vec3 angular_velocity;
+	glm::quat rotation;
 
 	bool world_collision;
 	bool remove;
-- 
2.39.2