From d832e19dab9bb852186f68866130dab758ce32ae Mon Sep 17 00:00:00 2001
From: bloeys <bloeys@outlook.com>
Date: Thu, 13 Jan 2022 15:49:30 +0400
Subject: [PATCH] Rotate and Col functions

---
 gglm/mat2.go      |  4 ++++
 gglm/mat3.go      |  4 ++++
 gglm/mat4.go      |  4 ++++
 gglm/transform.go | 46 ++++++++++++++++++++++++++++++++++++++++++++--
 4 files changed, 56 insertions(+), 2 deletions(-)

diff --git a/gglm/mat2.go b/gglm/mat2.go
index 9ec3807..cb393d0 100755
--- a/gglm/mat2.go
+++ b/gglm/mat2.go
@@ -28,6 +28,10 @@ func (m *Mat2) String() string {
 	return fmt.Sprintf("\n| %+-9.3f %+-9.3f |\n| %+-9.3f %+-9.3f |\n", m.Data[0][0], m.Data[0][1], m.Data[1][0], m.Data[1][1])
 }
 
+func (m *Mat2) Col(c int) *Vec2 {
+	return &Vec2{Data: m.Data[c]}
+}
+
 //Add m += m2
 func (m *Mat2) Add(m2 *Mat2) *Mat2 {
 	m.Data[0][0] += m2.Data[0][0]
diff --git a/gglm/mat3.go b/gglm/mat3.go
index 5cb4b4a..080fe71 100755
--- a/gglm/mat3.go
+++ b/gglm/mat3.go
@@ -31,6 +31,10 @@ func (m *Mat3) String() string {
 	)
 }
 
+func (m *Mat3) Col(c int) *Vec3 {
+	return &Vec3{Data: m.Data[c]}
+}
+
 //Add m += m2
 func (m *Mat3) Add(m2 *Mat3) *Mat3 {
 
diff --git a/gglm/mat4.go b/gglm/mat4.go
index e9d46cf..a6d9de3 100755
--- a/gglm/mat4.go
+++ b/gglm/mat4.go
@@ -32,6 +32,10 @@ func (m *Mat4) String() string {
 	)
 }
 
+func (m *Mat4) Col(c int) *Vec4 {
+	return &Vec4{Data: m.Data[c]}
+}
+
 //Add m += m2
 func (m *Mat4) Add(m2 *Mat4) *Mat4 {
 
diff --git a/gglm/transform.go b/gglm/transform.go
index cffe40d..82a7c9c 100755
--- a/gglm/transform.go
+++ b/gglm/transform.go
@@ -13,20 +13,62 @@ type TrMat struct {
 	Mat4
 }
 
-//Translate adds the vector to the translation components of the transformation matrix
+//Translate adds v to the translation components of the transformation matrix
 func (t *TrMat) Translate(v *Vec3) {
 	t.Data[3][0] += v.Data[0]
 	t.Data[3][1] += v.Data[1]
 	t.Data[3][2] += v.Data[2]
 }
 
-//Scale multiplies the vector by the scale components of the transformation matrix
+//Scale multiplies the scale components of the transformation matrix by v
 func (t *TrMat) Scale(v *Vec3) {
 	t.Data[0][0] *= v.Data[0]
 	t.Data[1][1] *= v.Data[1]
 	t.Data[2][2] *= v.Data[2]
 }
 
+func (t *TrMat) Rotate(rads float32, axis *Vec3) {
+
+	s := Sin32(rads)
+	c := Cos32(rads)
+
+	axis = axis.Normalize()
+	temp := axis.Clone().Scale(1 - c)
+
+	rotate := TrMat{}
+	rotate.Data[0][0] = c + temp.Data[0]*axis.Data[0]
+	rotate.Data[0][1] = temp.Data[0]*axis.Data[1] + s*axis.Data[2]
+	rotate.Data[0][2] = temp.Data[0]*axis.Data[2] - s*axis.Data[1]
+
+	rotate.Data[1][0] = temp.Data[1]*axis.Data[0] - s*axis.Data[2]
+	rotate.Data[1][1] = c + temp.Data[1]*axis.Data[1]
+	rotate.Data[1][2] = temp.Data[1]*axis.Data[2] + s*axis.Data[0]
+
+	rotate.Data[2][0] = temp.Data[2]*axis.Data[0] + s*axis.Data[1]
+	rotate.Data[2][1] = temp.Data[2]*axis.Data[1] - s*axis.Data[0]
+	rotate.Data[2][2] = c + temp.Data[2]*axis.Data[2]
+
+	result := &Mat4{}
+	result.Data[0] = t.Col(0).Scale(rotate.Data[0][0]).
+		Add(t.Col(1).Scale(rotate.Data[0][1])).
+		Add(t.Col(2).Scale(rotate.Data[0][2])).
+		Data
+
+	result.Data[1] = t.Col(0).Scale(rotate.Data[1][0]).
+		Add(t.Col(1).Scale(rotate.Data[1][1])).
+		Add(t.Col(2).Scale(rotate.Data[1][2])).
+		Data
+
+	result.Data[2] = t.Col(0).Scale(rotate.Data[2][0]).
+		Add(t.Col(1).Scale(rotate.Data[2][1])).
+		Add(t.Col(2).Scale(rotate.Data[2][2])).
+		Data
+
+	t.Data[0] = result.Data[0]
+	t.Data[1] = result.Data[1]
+	t.Data[2] = result.Data[2]
+}
+
 func (t *TrMat) Mul(m *TrMat) *TrMat {
 	t.Mat4.Mul(&m.Mat4)
 	return t