From b62da21ef8053c55ba1a3fbe00fc2fa394cad562 Mon Sep 17 00:00:00 2001
From: aap <aap@papnet.eu>
Date: Sun, 22 Oct 2017 12:19:00 +0200
Subject: [PATCH] implemented matrix to quaternion conversion

---
 src/base.cpp | 34 ++++++++++++++++++++++++++++++++++
 src/rwbase.h |  1 +
 2 files changed, 35 insertions(+)

diff --git a/src/base.cpp b/src/base.cpp
index 0dfccf7..581b4c1 100755
--- a/src/base.cpp
+++ b/src/base.cpp
@@ -343,6 +343,40 @@ Matrix::transform(Matrix *mat, CombineOp op)
 	return this;
 }
 
+Quat
+Matrix::getRotation(void)
+{
+	Quat q = { 0.0f, 0.0f, 0.0f, 1.0f };
+	float32 tr = right.x + up.y + at.z;
+	float s;
+	if(tr > 0.0f){
+		s = sqrt(1.0f + tr) * 2.0f;
+		q.w = s / 4.0f;
+		q.x = (up.z - at.y) / s;
+		q.y = (at.x - right.z) / s;
+		q.z = (right.y - up.x) / s;
+	}else if(right.x > up.y && right.x > at.z){
+		s = sqrt(1.0f + right.x - up.y - at.z) * 2.0f;
+		q.w = (up.z - at.y) / s;
+		q.x = s / 4.0f;
+		q.y = (up.x + right.y) / s;
+		q.z = (at.x + right.z) / s;
+	}else if(up.y > at.z){
+		s = sqrt(1.0f + up.y - right.x - at.z) * 2.0f;
+		q.w = (at.x - right.z) / s;
+		q.x = (up.x + right.y) / s;
+		q.y = s / 4.0f;
+		q.z = (at.y + up.z) / s;
+	}else{
+		s = sqrt(1.0f + at.z - right.x - up.y) * 2.0f;
+		q.w = (right.y - up.x) / s;
+		q.x = (at.x + right.z) / s;
+		q.y = (at.y + up.z) / s;
+		q.z = s / 4.0f;
+	}
+	return q;
+}
+
 void
 Matrix::lookAt(const V3d &dir, const V3d &up)
 {
diff --git a/src/rwbase.h b/src/rwbase.h
index f13a287..3fc1927 100644
--- a/src/rwbase.h
+++ b/src/rwbase.h
@@ -321,6 +321,7 @@ struct Matrix
 	Matrix *translate(V3d *translation, CombineOp op);
 	Matrix *scale(V3d *scl, CombineOp op);
 	Matrix *transform(Matrix *mat, CombineOp op);
+	Quat getRotation(void);
 	void lookAt(const V3d &dir, const V3d &up);
 
 	// helper functions. consider private