From e4edb7dcecc589782b79176cbc4f6714c59cde72 Mon Sep 17 00:00:00 2001
From: bloeys <bloeys@outlook.com>
Date: Thu, 13 Jan 2022 16:06:13 +0400
Subject: [PATCH] Angle and axis methods for quat

---
 gglm/constants.go |  3 +++
 gglm/quat.go      | 31 +++++++++++++++++++++++++++++++
 gglm/scalar.go    |  8 ++++++++
 3 files changed, 42 insertions(+)

diff --git a/gglm/constants.go b/gglm/constants.go
index 94a0439..b789bc4 100755
--- a/gglm/constants.go
+++ b/gglm/constants.go
@@ -4,4 +4,7 @@ const (
 	Pi      float32 = 3.14159265359
 	Deg2Rad float32 = Pi / 180
 	Rad2Deg float32 = 180 / Pi
+
+	//CosHalf is Cos32(0.5)
+	CosHalf float32 = 0.87758256189
 )
diff --git a/gglm/quat.go b/gglm/quat.go
index f1c1b30..644b3bf 100755
--- a/gglm/quat.go
+++ b/gglm/quat.go
@@ -16,6 +16,37 @@ func (q *Quat) Eq(q2 *Quat) bool {
 	return q.Data == q2.Data
 }
 
+//Angle returns the angle represented by this quaternion
+func (q *Quat) Angle() float32 {
+
+	if Abs32(q.Data[3]) > CosHalf {
+
+		a := Asin32(Sqrt32(q.Data[0]*q.Data[0]+q.Data[1]*q.Data[1]+q.Data[2]*q.Data[2])) * 2
+		if q.Data[3] < 0 {
+			return Pi*2 - a
+		}
+		return a
+	}
+
+	return Acos32(q.Data[3]) * 2
+}
+
+//Axis returns the rotation axis represented by this quaternion
+func (q *Quat) Axis() *Vec3 {
+
+	var t float32 = 1 - q.Data[3]*q.Data[3]
+	if t <= 0 {
+		return &Vec3{Data: [3]float32{0, 0, 1}}
+	}
+
+	t = 1 / Sqrt32(t)
+	return &Vec3{Data: [3]float32{
+		q.Data[0] * t,
+		q.Data[1] * t,
+		q.Data[2] * t,
+	}}
+}
+
 //Euler takes rotations in radians and produces a rotation that
 //rotates around the z-axis, y-axis and lastly x-axis.
 func NewQuatEuler(v *Vec3) *Quat {
diff --git a/gglm/scalar.go b/gglm/scalar.go
index 4fb79ec..55313a2 100755
--- a/gglm/scalar.go
+++ b/gglm/scalar.go
@@ -35,3 +35,11 @@ func Sincos32(x float32) (sinx, cosx float32) {
 	a, b := math.Sincos(float64(x))
 	return float32(a), float32(b)
 }
+
+func Abs32(x float32) float32 {
+	return float32(math.Abs(float64(x)))
+}
+
+func Sqrt32(x float32) float32 {
+	return float32(math.Sqrt(float64(x)))
+}