Mat4.InvertAndTranspose()+ ToMat3 and ToMat2 functions

.gitignore

@@ -12,4 +12,6 @@
 *.out
 
 # Dependency directories (remove the comment below to include it)
-# vendor/
+vendor/
+.vscode
+*~

README.md

@@ -1,2 +1,56 @@
 # gglm
-An OpenGL focused Go mathematics inspired by the C++ glm (OpenGL Mathematics) library
+
+Fast OpenGL/Graphics focused Mathematics library in Go inspired by the c++ library [glm](https://github.com/g-truc/glm).
+
+gglm currently has the following:
+
+- Matrices are stored column major
+- `Vec2`, `Vec3` and `Vec4` structs that implement vector (x,y,z,w) operations
+- `Mat2`, `Mat3`, `Mat4` structs that implement square matrix operations
+- `Quat` struct that implements quaternion operations
+- `TrMat` struct that implements 3D transformation matrix operations
+- Many useful geometric functions (e.g. dot product, cross product, vector reflection etc)
+- 32-bit scalar operations (e.g. sin32, cos32, equality using epsilon, sqrt32 etc)
+- Useful 32-bit constants (e.g. pi, Deg2Rad, Rad2Deg, float32 epsilon etc)
+- Simple 'swizzle' interfaces that allow you to do things like `.X()` or `.R()` etc.
+- Very easy to use with graphics/native APIs as everything is implemented using arrays
+- `.String()` functions on all types for pretty printing
+
+## Installation
+
+`go get github.com/bloeys/gglm`
+
+## Usage
+
+```go
+import "github.com/bloeys/gglm/gglm"
+
+func main() {
+	
+	// Vec2
+	v1 := &gglm.Vec2{Data: [2]float32{1, 2}}
+	v2 := &gglm.Vec2{Data: [2]float32{3, 4}}
+	println(gglm.DistVec2(v1, v2))
+	println(gglm.SqrDistVec2(v2, v1))
+	println(v1.Eq(v2))
+	v2.Set(1, 2)
+	println(v1.Eq(v2))
+
+	// This performs: v1 += v2
+	// v1 is returned from the function, so we can chain calls that operate on v1
+	newX := v1.Add(v2).X()
+	println("newX:", newX)
+
+	// LookAt for a right-handed coordinate system
+	camPos := gglm.NewVec3(0, 0, 3)
+	worldUp := gglm.NewVec3(0, 1, 0)
+	targetPos := gglm.NewVec3(0, 0, 0)
+	viewMat := gglm.LookAtRH(camPos, targetPos, worldUp)
+	println(viewMat.String())
+}
+```
+
+## Notes
+
+You can check compiler inlining decisions using `go run -gcflags "-m" .`. Some functions look a bit weird compared to similar ones
+because we are trying to reduce function complexity so the compiler inlines.

gglm/constants.go

@@ -0,0 +1,11 @@
+package gglm
+
+const (
+	Pi         float32 = 3.14159265359
+	Deg2Rad    float32 = Pi / 180
+	Rad2Deg    float32 = 180 / Pi
+	F32Epsilon float32 = 1e-6
+
+	// CosHalf is Cos32(0.5)
+	CosHalf float32 = 0.87758256189
+)

gglm/geometric.go

@@ -1,5 +1,7 @@
 package gglm
 
+import "math"
+
 func DotVec2(v1, v2 *Vec2) float32 {
 	return v1.X()*v2.X() + v1.Y()*v2.Y()
 }
@@ -8,6 +10,14 @@ func DotVec3(v1, v2 *Vec3) float32 {
 	return v1.X()*v2.X() + v1.Y()*v2.Y() + v1.Z()*v2.Z()
 }
 
+func DotVec4(v1, v2 *Vec4) float32 {
+	return v1.X()*v2.X() + v1.Y()*v2.Y() + v1.Z()*v2.Z() + v1.W()*v2.W()
+}
+
+func DotQuat(q1, q2 *Quat) float32 {
+	return q1.X()*q2.X() + q1.Y()*q2.Y() + q1.Z()*q2.Z() + q1.W()*q2.W()
+}
+
 func Cross(v1, v2 *Vec3) *Vec3 {
 	return &Vec3{
 		Data: [3]float32{
@@ -17,3 +27,96 @@ func Cross(v1, v2 *Vec3) *Vec3 {
 		},
 	}
 }
+
+// DistVec2 returns euclidean distance between v1 and v2
+func DistVec2(v1, v2 *Vec2) float32 {
+	x := v1.X() - v2.X()
+	y := v1.Y() - v2.Y()
+	return float32(math.Sqrt(float64(x*x + y*y)))
+}
+
+// DistVec3 returns euclidean distance between v1 and v2
+func DistVec3(v1, v2 *Vec3) float32 {
+	x := v1.X() - v2.X()
+	y := v1.Y() - v2.Y()
+	z := v1.Z() - v2.Z()
+	return float32(math.Sqrt(float64(x*x + y*y + z*z)))
+}
+
+// DistVec4 returns euclidean distance between v1 and v2
+func DistVec4(v1, v2 *Vec4) float32 {
+
+	//Using X() etc won't let the function inline
+	x := v1.Data[0] - v2.Data[0]
+	y := v1.Data[1] - v2.Data[1]
+	z := v1.Data[2] - v2.Data[2]
+	w := v1.Data[3] - v2.Data[3]
+	return float32(math.Sqrt(float64(x*x + y*y + z*z + w*w)))
+}
+
+// DistVec2 returns the squared euclidean distance between v1 and v2 (avoids a sqrt)
+func SqrDistVec2(v1, v2 *Vec2) float32 {
+	x := v1.X() - v2.X()
+	y := v1.Y() - v2.Y()
+	return x*x + y*y
+}
+
+// DistVec3 returns the squared euclidean distance between v1 and v2 (avoids a sqrt)
+func SqrDistVec3(v1, v2 *Vec3) float32 {
+	x := v1.X() - v2.X()
+	y := v1.Y() - v2.Y()
+	z := v1.Z() - v2.Z()
+	return x*x + y*y + z*z
+}
+
+// DistVec4 returns the squared euclidean distance between v1 and v2 (avoids a sqrt)
+func SqrDistVec4(v1, v2 *Vec4) float32 {
+	x := v1.Data[0] - v2.Data[0]
+	y := v1.Data[1] - v2.Data[1]
+	z := v1.Data[2] - v2.Data[2]
+	w := v1.Data[3] - v2.Data[3]
+	return x*x + y*y + z*z + w*w
+}
+
+// ReflectVec2 returns the reflected vector of the incoming vector 'v', and the surface normal 'n'.
+//
+// Note: n must be normalized or you will get wrong results
+func ReflectVec2(v, n *Vec2) *Vec2 {
+
+	//reflectedVec = v − 2*dot(v, norm)*norm
+	d := 2 * (v.Data[0]*n.Data[0] + v.Data[1]*n.Data[1])
+
+	return &Vec2{
+		Data: [2]float32{
+			v.Data[0] - d*n.Data[0],
+			v.Data[1] - d*n.Data[1],
+		},
+	}
+}
+
+// ReflectVec3 returns the reflected vector of the incoming vector 'v', and the surface normal 'n'.
+//
+// Note: n must be normalized or you will get wrong results
+func ReflectVec3(v, n *Vec3) *Vec3 {
+
+	//reflectedVec = v − 2*dot(v, norm)*norm
+	d := 2 * (v.Data[0]*n.Data[0] + v.Data[1]*n.Data[1] + v.Data[2]*n.Data[2])
+
+	return &Vec3{
+		Data: [3]float32{
+			v.Data[0] - d*n.Data[0],
+			v.Data[1] - d*n.Data[1],
+			v.Data[2] - d*n.Data[2],
+		},
+	}
+}
+
+// AngleVec3 returns the angle between the two vectors in radians
+func AngleVec3(v1, v2 *Vec3) float32 {
+	return Acos32(DotVec3(v1, v2) / (v1.Mag() * v2.Mag()))
+}
+
+// AngleQuat returns the angle between the two quaternions in radians
+func AngleQuat(q1, q2 *Quat) float32 {
+	return Acos32(DotQuat(q1, q2))
+}

gglm/geometric_test.go

@@ -0,0 +1,216 @@
+package gglm_test
+
+import (
+	"testing"
+
+	"github.com/bloeys/gglm/gglm"
+)
+
+var (
+	dotVec2Result, distVec2Result  float32
+	dotVec3Result, distVec3Result  float32
+	reflectVec2Result              *gglm.Vec2
+	crossResult, reflectVec3Result *gglm.Vec3
+)
+
+func TestDotVec2(t *testing.T) {
+
+	v1 := gglm.Vec2{Data: [2]float32{1, 2}}
+	v2 := gglm.Vec2{Data: [2]float32{3, 4}}
+	ans := float32(11)
+
+	res := gglm.DotVec2(&v1, &v2)
+	if res != ans {
+		t.Errorf("Got: %v; Expected: %v", res, ans)
+	}
+}
+
+func TestDotVec3(t *testing.T) {
+
+	v1 := gglm.Vec3{Data: [3]float32{1, 2, 3}}
+	v2 := gglm.Vec3{Data: [3]float32{4, 5, 6}}
+	ans := float32(32)
+
+	res := gglm.DotVec3(&v1, &v2)
+	if res != ans {
+		t.Errorf("Got: %v; Expected: %v", res, ans)
+	}
+}
+
+func TestDotVec4(t *testing.T) {
+
+	v1 := gglm.Vec4{Data: [4]float32{1, 2, 3, 4}}
+	v2 := gglm.Vec4{Data: [4]float32{5, 6, 7, 8}}
+	ans := float32(70)
+
+	res := gglm.DotVec4(&v1, &v2)
+	if res != ans {
+		t.Errorf("Got: %v; Expected: %v", res, ans)
+	}
+}
+
+func TestDistVec2(t *testing.T) {
+
+	v1 := gglm.Vec2{Data: [2]float32{1, 2}}
+	v2 := gglm.Vec2{Data: [2]float32{3, 4}}
+	ans := float32(2.828427)
+
+	res := gglm.DistVec2(&v1, &v2)
+	if res != ans {
+		t.Errorf("Got: %v; Expected: %v", res, ans)
+	}
+}
+
+func TestDistVec3(t *testing.T) {
+
+	v1 := gglm.Vec3{Data: [3]float32{1, 2, 3}}
+	v2 := gglm.Vec3{Data: [3]float32{4, 5, 6}}
+	ans := float32(5.196152)
+
+	res := gglm.DistVec3(&v1, &v2)
+	if res != ans {
+		t.Errorf("Got: %v; Expected: %v", res, ans)
+	}
+}
+
+func TestDistVec4(t *testing.T) {
+
+	v1 := gglm.Vec4{Data: [4]float32{1, 2, 3, 4}}
+	v2 := gglm.Vec4{Data: [4]float32{5, 6, 7, 8}}
+	ans := float32(8)
+
+	res := gglm.DistVec4(&v1, &v2)
+	if res != ans {
+		t.Errorf("Got: %v; Expected: %v", res, ans)
+	}
+}
+
+func TestSqrDistVec2(t *testing.T) {
+
+	v1 := gglm.Vec2{Data: [2]float32{1, 2}}
+	v2 := gglm.Vec2{Data: [2]float32{3, 4}}
+	ans := float32(8)
+
+	res := gglm.SqrDistVec2(&v1, &v2)
+	if res != ans {
+		t.Errorf("Got: %v; Expected: %v", res, ans)
+	}
+}
+
+func TestSqrDistVec3(t *testing.T) {
+
+	v1 := gglm.Vec3{Data: [3]float32{1, 2, 3}}
+	v2 := gglm.Vec3{Data: [3]float32{4, 5, 6}}
+	ans := float32(27)
+
+	res := gglm.SqrDistVec3(&v1, &v2)
+	if res != ans {
+		t.Errorf("Got: %v; Expected: %v", res, ans)
+	}
+}
+
+func TestSqrDistVec4(t *testing.T) {
+
+	v1 := gglm.Vec4{Data: [4]float32{1, 2, 3, 4}}
+	v2 := gglm.Vec4{Data: [4]float32{5, 6, 7, 8}}
+	ans := float32(64)
+
+	res := gglm.SqrDistVec4(&v1, &v2)
+	if res != ans {
+		t.Errorf("Got: %v; Expected: %v", res, ans)
+	}
+}
+
+func TestReflectVec2(t *testing.T) {
+
+	v1 := gglm.Vec2{Data: [2]float32{1, 2}}
+	n := gglm.Vec2{Data: [2]float32{0, 1}}
+	ans := gglm.Vec2{Data: [2]float32{1, -2}}
+
+	res := gglm.ReflectVec2(&v1, &n)
+	if !res.Eq(&ans) {
+		t.Errorf("Got: %v; Expected: %v", res, ans)
+	}
+}
+
+func TestReflectVec3(t *testing.T) {
+
+	v1 := gglm.Vec3{Data: [3]float32{1, 2, 3}}
+	n := gglm.Vec3{Data: [3]float32{0, 1, 0}}
+	ans := gglm.Vec3{Data: [3]float32{1, -2, 3}}
+
+	res := gglm.ReflectVec3(&v1, &n)
+	if !res.Eq(&ans) {
+		t.Errorf("Got: %v; Expected: %v", res, ans)
+	}
+}
+
+func BenchmarkDotVec2(b *testing.B) {
+
+	v1 := &gglm.Vec2{}
+	v2 := &gglm.Vec2{}
+
+	for i := 0; i < b.N; i++ {
+		dotVec2Result = gglm.DotVec2(v1, v2)
+	}
+}
+
+func BenchmarkDotVec3(b *testing.B) {
+
+	v1 := &gglm.Vec3{}
+	v2 := &gglm.Vec3{}
+
+	for i := 0; i < b.N; i++ {
+		dotVec3Result = gglm.DotVec3(v1, v2)
+	}
+}
+
+func BenchmarkCross(b *testing.B) {
+
+	v1 := &gglm.Vec3{}
+	v2 := &gglm.Vec3{}
+
+	for i := 0; i < b.N; i++ {
+		crossResult = gglm.Cross(v1, v2)
+	}
+}
+
+func BenchmarkDistVec2(b *testing.B) {
+
+	v1 := &gglm.Vec2{}
+	v2 := &gglm.Vec2{}
+
+	for i := 0; i < b.N; i++ {
+		distVec2Result = gglm.DistVec2(v1, v2)
+	}
+}
+
+func BenchmarkDistVec3(b *testing.B) {
+
+	v1 := &gglm.Vec3{}
+	v2 := &gglm.Vec3{}
+
+	for i := 0; i < b.N; i++ {
+		distVec3Result = gglm.DistVec3(v1, v2)
+	}
+}
+
+func BenchmarkReflectVec2(b *testing.B) {
+
+	v1 := &gglm.Vec2{}
+	v2 := &gglm.Vec2{}
+
+	for i := 0; i < b.N; i++ {
+		reflectVec2Result = gglm.ReflectVec2(v1, v2)
+	}
+}
+
+func BenchmarkReflectVec3(b *testing.B) {
+
+	v1 := &gglm.Vec3{}
+	v2 := &gglm.Vec3{}
+
+	for i := 0; i < b.N; i++ {
+		reflectVec3Result = gglm.ReflectVec3(v1, v2)
+	}
+}

gglm/mat.go

@@ -13,7 +13,7 @@ const (
 	MatSize4x4
 )
 
-//String panics if the MatSize is not known
+// String panics if the MatSize is not known
 func (ms MatSize) String() string {
 
 	switch ms {
@@ -29,7 +29,7 @@ func (ms MatSize) String() string {
 }
 
 type Mat interface {
-	At(row, col int) float32
+	Get(row, col int) float32
 	Set(row, col int, val float32)
 	Size() MatSize
 }

gglm/mat2.go

@@ -8,15 +8,15 @@ var _ Mat = &Mat2{}
 var _ fmt.Stringer = &Mat2{}
 
 type Mat2 struct {
-	Data [4]float32
+	Data [2][2]float32
 }
 
-func (m *Mat2) At(row, col int) float32 {
-	return m.Data[row*2+col]
+func (m *Mat2) Get(row, col int) float32 {
+	return m.Data[col][row]
 }
 
 func (m *Mat2) Set(row, col int, val float32) {
-	m.Data[row*2+col] = val
+	m.Data[col][row] = val
 }
 
 func (m *Mat2) Size() MatSize {
@@ -24,88 +24,195 @@ func (m *Mat2) Size() MatSize {
 }
 
 func (m *Mat2) String() string {
-	//+ always shows +/- sign; - means pad to the right; 9 means total of 9 digits (or padding if less); .3 means 3 decimals
-	return fmt.Sprintf("\n| %+-9.3f %+-9.3f |\n| %+-9.3f %+-9.3f |\n", m.Data[0], m.Data[1], m.Data[2], m.Data[3])
+	// + always shows +/- sign; - means pad to the right; 9 means total of 9 digits (or padding if less); .3 means 3 decimals
+	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])
 }
 
-//Add m += m2
-func (m *Mat2) Add(m2 *Mat2) {
-	m.Data[0] += m2.Data[0]
-	m.Data[1] += m2.Data[1]
-	m.Data[2] += m2.Data[2]
-	m.Data[3] += m2.Data[3]
+func (m *Mat2) Col(c int) *Vec2 {
+	return &Vec2{Data: m.Data[c]}
 }
 
-//Add m -= m2
-func (m *Mat2) Sub(m2 *Mat2) {
-	m.Data[0] -= m2.Data[0]
-	m.Data[1] -= m2.Data[1]
-	m.Data[2] -= m2.Data[2]
-	m.Data[3] -= m2.Data[3]
+// Add m += m2
+func (m *Mat2) Add(m2 *Mat2) *Mat2 {
+	m.Data[0][0] += m2.Data[0][0]
+	m.Data[0][1] += m2.Data[0][1]
+	m.Data[1][0] += m2.Data[1][0]
+	m.Data[1][1] += m2.Data[1][1]
+	return m
 }
 
-//Mul m *= m2
-func (m *Mat2) Mul(m2 *Mat2) {
-	m.Data = [4]float32{
-		m.Data[0]*m2.Data[0] + m.Data[1]*m2.Data[2],
-		m.Data[0]*m2.Data[1] + m.Data[1]*m2.Data[3],
+// Add m -= m2
+func (m *Mat2) Sub(m2 *Mat2) *Mat2 {
+	m.Data[0][0] -= m2.Data[0][0]
+	m.Data[0][1] -= m2.Data[0][1]
+	m.Data[1][0] -= m2.Data[1][0]
+	m.Data[1][1] -= m2.Data[1][1]
+	return m
+}
 
-		m.Data[2]*m2.Data[0] + m.Data[3]*m2.Data[2],
-		m.Data[2]*m2.Data[1] + m.Data[3]*m2.Data[3],
+// Mul m *= m2
+func (m1 *Mat2) Mul(m2 *Mat2) *Mat2 {
+	m1.Data = [2][2]float32{
+		{
+			m1.Data[0][0]*m2.Data[0][0] + m1.Data[1][0]*m2.Data[0][1],
+			m1.Data[0][1]*m2.Data[0][0] + m1.Data[1][1]*m2.Data[0][1],
+		},
+		{
+			m1.Data[0][0]*m2.Data[1][0] + m1.Data[1][0]*m2.Data[1][1],
+			m1.Data[0][1]*m2.Data[1][0] + m1.Data[1][1]*m2.Data[1][1],
+		},
 	}
+
+	return m1
 }
 
-//Scale m *= x (element wise multiplication)
-func (m *Mat2) Scale(x float32) {
-	m.Data[0] *= x
-	m.Data[1] *= x
-	m.Data[2] *= x
-	m.Data[3] *= x
+// Scale m *= x (element wise multiplication)
+func (m *Mat2) Scale(x float32) *Mat2 {
+	m.Data[0][0] *= x
+	m.Data[0][1] *= x
+	m.Data[1][0] *= x
+	m.Data[1][1] *= x
+	return m
 }
 
-//AddMat2 m3 = m1 + m2
+func (m *Mat2) Clone() *Mat2 {
+	return &Mat2{Data: m.Data}
+}
+
+func (m *Mat2) Eq(m2 *Mat2) bool {
+	return m.Data == m2.Data
+}
+
+func (m *Mat2) Transpose() *Mat2 {
+	m.Data = [2][2]float32{
+		{m.Data[0][0], m.Data[1][0]},
+		{m.Data[0][1], m.Data[1][1]},
+	}
+
+	return m
+}
+
+func (m *Mat2) Determinant() float32 {
+	return (m.Data[0][0] * m.Data[1][1]) - (m.Data[0][1] * m.Data[1][0])
+}
+
+// Invert inverts this matrix.
+//
+// Note that the inverse is not defined if the determinant is zero or extremely small.
+// In the case the determinant is zero the matrix will (usually) get filled with infinities
+func (m *Mat2) Invert() *Mat2 {
+
+	// https://www.cuemath.com/algebra/inverse-of-2x2-matrix/
+	inverseDet := 1 / m.Determinant()
+
+	m.Data = [2][2]float32{
+		{m.Data[1][1] * inverseDet, -m.Data[0][1] * inverseDet}, // Col0
+		{-m.Data[1][0] * inverseDet, m.Data[0][0] * inverseDet}, // Col1
+	}
+
+	return m
+}
+
+// AddMat2 m3 = m1 + m2
 func AddMat2(m1, m2 *Mat2) *Mat2 {
 	return &Mat2{
-		Data: [4]float32{
-			m1.Data[0] + m2.Data[0],
-			m1.Data[1] + m2.Data[1],
-			m1.Data[2] + m2.Data[2],
-			m1.Data[3] + m2.Data[3],
+		Data: [2][2]float32{
+			{
+				m1.Data[0][0] + m2.Data[0][0],
+				m1.Data[0][1] + m2.Data[0][1],
+			},
+			{
+				m1.Data[1][0] + m2.Data[1][0],
+				m1.Data[1][1] + m2.Data[1][1],
+			},
 		},
 	}
 }
 
-//SubMat2 m3 = m1 - m2
+// SubMat2 m3 = m1 - m2
 func SubMat2(m1, m2 *Mat2) *Mat2 {
 	return &Mat2{
-		Data: [4]float32{
-			m1.Data[0] - m2.Data[0],
-			m1.Data[1] - m2.Data[1],
-			m1.Data[2] - m2.Data[2],
-			m1.Data[3] - m2.Data[3],
+		Data: [2][2]float32{
+			{
+				m1.Data[0][0] - m2.Data[0][0],
+				m1.Data[0][1] - m2.Data[0][1],
+			},
+			{
+				m1.Data[1][0] - m2.Data[1][0],
+				m1.Data[1][1] - m2.Data[1][1],
+			},
 		},
 	}
 }
 
-//MulMat2 m3 = m1 * m2
+// MulMat2 m3 = m1 * m2
 func MulMat2(m1, m2 *Mat2) *Mat2 {
 	return &Mat2{
-		Data: [4]float32{
-			m1.Data[0]*m2.Data[0] + m1.Data[1]*m2.Data[2],
-			m1.Data[0]*m2.Data[1] + m1.Data[1]*m2.Data[3],
-
-			m1.Data[2]*m2.Data[0] + m1.Data[3]*m2.Data[2],
-			m1.Data[2]*m2.Data[1] + m1.Data[3]*m2.Data[3],
+		Data: [2][2]float32{
+			{
+				m1.Data[0][0]*m2.Data[0][0] + m1.Data[1][0]*m2.Data[0][1],
+				m1.Data[0][1]*m2.Data[0][0] + m1.Data[1][1]*m2.Data[0][1],
+			},
+			{
+				m1.Data[0][0]*m2.Data[1][0] + m1.Data[1][0]*m2.Data[1][1],
+				m1.Data[0][1]*m2.Data[1][0] + m1.Data[1][1]*m2.Data[1][1],
+			},
 		},
 	}
 }
 
-//NewMat2Id returns the 2x2 identity matrix
+// MulMat2Vec2 v2 = m1 * v1
+func MulMat2Vec2(m1 *Mat2, v1 *Vec2) *Vec2 {
+	return &Vec2{
+		Data: [2]float32{
+			m1.Data[0][0]*v1.Data[0] + m1.Data[1][0]*v1.Data[1],
+			m1.Data[0][1]*v1.Data[0] + m1.Data[1][1]*v1.Data[1],
+		},
+	}
+}
+
+// NewMat2Id returns the 2x2 identity matrix
 func NewMat2Id() *Mat2 {
 	return &Mat2{
-		Data: [4]float32{
-			1, 0,
-			0, 1,
+		Data: [2][2]float32{
+			{1, 0},
+			{0, 1},
+		},
+	}
+}
+
+func NewMat2Diag(diagVal float32) *Mat2 {
+	return &Mat2{
+		Data: [2][2]float32{
+			{diagVal, 0},
+			{0, diagVal},
+		},
+	}
+}
+
+func NewMat2DiagArr(diag [2]float32) *Mat2 {
+	return &Mat2{
+		Data: [2][2]float32{
+			{diag[0], 0},
+			{0, diag[1]},
+		},
+	}
+}
+
+func NewMat2Vec2(col0, col1 *Vec2) *Mat2 {
+	return &Mat2{
+		Data: [2][2]float32{
+			col0.Data,
+			col1.Data,
+		},
+	}
+}
+
+func NewMat2Arr(col0, col1 [2]float32) *Mat2 {
+	return &Mat2{
+		Data: [2][2]float32{
+			col0,
+			col1,
 		},
 	}
 }

gglm/mat2_test.go

@@ -0,0 +1,240 @@
+package gglm_test
+
+import (
+	"testing"
+
+	"github.com/bloeys/gglm/gglm"
+)
+
+func TestMat2GetSet(t *testing.T) {
+
+	m1 := gglm.Mat2{}
+
+	m1.Set(0, 1, -10)
+	m1.Set(1, 0, 55)
+
+	if m1.Get(0, 1) != -10 {
+		t.Errorf("Got: %v; Expected: %v", m1.Get(0, 1), -10)
+	}
+
+	if m1.Get(1, 0) != 55 {
+		t.Errorf("Got: %v; Expected: %v", m1.Get(1, 0), 55)
+	}
+}
+
+func TestMat2Id(t *testing.T) {
+
+	correctAns := gglm.Mat2{
+		Data: [2][2]float32{
+			{1, 0},
+			{0, 1},
+		}}
+
+	m1 := gglm.NewMat2Id()
+	if !m1.Eq(&correctAns) {
+		t.Errorf("Got: %v; Expected: %v", m1.String(), correctAns.String())
+	}
+}
+
+func TestSubMat2(t *testing.T) {
+
+	correctAns := gglm.Mat2{
+		Data: [2][2]float32{
+			{-4, -4},
+			{-4, -4},
+		}}
+
+	m1 := &gglm.Mat2{
+		Data: [2][2]float32{
+			{1, 3},
+			{2, 4},
+		}}
+	m2 := &gglm.Mat2{
+		Data: [2][2]float32{
+			{5, 7},
+			{6, 8},
+		}}
+
+	result := gglm.SubMat2(m1, m2)
+	m1.Sub(m2)
+
+	if !result.Eq(&correctAns) {
+		t.Errorf("Got: %v; Expected: %v", result.String(), correctAns.String())
+	}
+
+	if !m1.Eq(&correctAns) {
+		t.Errorf("Got: %v; Expected: %v", m1.String(), correctAns.String())
+	}
+}
+
+func TestAddMat2(t *testing.T) {
+
+	correctAns := gglm.Mat2{
+		Data: [2][2]float32{
+			{6, 10},
+			{8, 12},
+		}}
+
+	m1 := &gglm.Mat2{
+		Data: [2][2]float32{
+			{1, 3},
+			{2, 4},
+		}}
+	m2 := &gglm.Mat2{
+		Data: [2][2]float32{
+			{5, 7},
+			{6, 8},
+		}}
+
+	result := gglm.AddMat2(m1, m2)
+	m1.Add(m2)
+
+	if !result.Eq(&correctAns) {
+		t.Errorf("Got: %v; Expected: %v", result.String(), correctAns.String())
+	}
+
+	if !m1.Eq(&correctAns) {
+		t.Errorf("Got: %v; Expected: %v", m1.String(), correctAns.String())
+	}
+}
+
+func TestMulMat2(t *testing.T) {
+
+	correctAns := gglm.Mat2{
+		Data: [2][2]float32{
+			{19, 43},
+			{22, 50},
+		}}
+
+	m1 := &gglm.Mat2{
+		Data: [2][2]float32{
+			{1, 3},
+			{2, 4},
+		}}
+	m2 := &gglm.Mat2{
+		Data: [2][2]float32{
+			{5, 7},
+			{6, 8},
+		}}
+
+	result := gglm.MulMat2(m1, m2)
+	m1.Mul(m2)
+
+	if !result.Eq(&correctAns) {
+		t.Errorf("Got: %v; Expected: %v", result.String(), correctAns.String())
+	}
+
+	if !m1.Eq(&correctAns) {
+		t.Errorf("Got: %v; Expected: %v", m1.String(), correctAns.String())
+	}
+}
+
+func TestMulMat2Vec2(t *testing.T) {
+
+	correctAns := gglm.Vec2{Data: [2]float32{5, 11}}
+
+	m := &gglm.Mat2{
+		Data: [2][2]float32{
+			{1, 3},
+			{2, 4},
+		}}
+	v := &gglm.Vec2{Data: [2]float32{1, 2}}
+
+	result := gglm.MulMat2Vec2(m, v)
+
+	if !result.Eq(&correctAns) {
+		t.Errorf("Got: %v; Expected: %v", result.String(), correctAns.String())
+	}
+}
+
+func TestTransposeMat2(t *testing.T) {
+
+	m := gglm.NewMat2Id()
+	ans := gglm.NewMat2Id()
+
+	if !m.Transpose().Transpose().Eq(ans) {
+		t.Errorf("Got: %v; Expected: %v", m.String(), ans.String())
+	}
+
+	if !m.Transpose().Eq(ans) {
+		t.Errorf("Got: %v; Expected: %v", m.String(), ans.String())
+	}
+
+	m = &gglm.Mat2{
+		Data: [2][2]float32{
+			{00, 01},
+			{10, 11},
+		},
+	}
+
+	ans = &gglm.Mat2{
+		Data: [2][2]float32{
+			{00, 10},
+			{01, 11},
+		},
+	}
+
+	if !m.Transpose().Eq(ans) {
+		t.Errorf("Got: %v; Expected: %v", m.String(), ans.String())
+	}
+}
+
+func TestDeterminantMat2(t *testing.T) {
+
+	m := gglm.NewMat2Id()
+	ans := float32(1)
+
+	if m.Determinant() != ans {
+		t.Errorf("Got: %f; Expected: %f", m.Determinant(), ans)
+	}
+
+	m = &gglm.Mat2{
+		Data: [2][2]float32{
+			{1, 8},
+			{5, 31},
+		},
+	}
+	ans = -9
+
+	if m.Determinant() != ans {
+		t.Errorf("Got: %f; Expected: %f", m.Determinant(), ans)
+	}
+}
+
+func TestInvertMat2(t *testing.T) {
+
+	m := gglm.NewMat2Id()
+	ans := gglm.NewMat2Id()
+
+	if !m.Invert().Eq(ans) {
+		t.Errorf("Got: %v; Expected: %v", m.String(), ans.String())
+	}
+
+	m = &gglm.Mat2{
+		Data: [2][2]float32{
+			{1, 8},
+			{5, 31},
+		},
+	}
+
+	ans = &gglm.Mat2{
+		Data: [2][2]float32{
+			{-31 / 9.0, 8 / 9.0},
+			{5 / 9.0, -1 / 9.0},
+		},
+	}
+
+	if !m.Invert().Eq(ans) {
+		t.Errorf("Got: %v; Expected: %v", m.String(), ans.String())
+	}
+}
+
+func BenchmarkMulMat2(b *testing.B) {
+
+	m1 := gglm.NewMat2Id()
+	m2 := gglm.NewMat2Id()
+
+	for i := 0; i < b.N; i++ {
+		m1.Mul(m2)
+	}
+}

gglm/mat3.go

@@ -8,15 +8,15 @@ var _ Mat = &Mat3{}
 var _ fmt.Stringer = &Mat3{}
 
 type Mat3 struct {
-	Data [9]float32
+	Data [3][3]float32
 }
 
-func (m *Mat3) At(row, col int) float32 {
-	return m.Data[row*3+col]
+func (m *Mat3) Get(row, col int) float32 {
+	return m.Data[col][row]
 }
 
 func (m *Mat3) Set(row, col int, val float32) {
-	m.Data[row*3+col] = val
+	m.Data[col][row] = val
 }
 
 func (m *Mat3) Size() MatSize {
@@ -25,147 +25,325 @@ func (m *Mat3) Size() MatSize {
 
 func (m *Mat3) String() string {
 	return fmt.Sprintf("\n| %+-9.3f %+-9.3f %+-9.3f |\n| %+-9.3f %+-9.3f %+-9.3f |\n| %+-9.3f %+-9.3f %+-9.3f |\n",
-		m.Data[0], m.Data[1], m.Data[2],
-		m.Data[3], m.Data[4], m.Data[5],
-		m.Data[6], m.Data[7], m.Data[8],
+		m.Data[0][0], m.Data[0][1], m.Data[0][2],
+		m.Data[1][0], m.Data[1][1], m.Data[1][2],
+		m.Data[2][0], m.Data[2][1], m.Data[2][2],
 	)
 }
 
-//Add m += m2
-func (m *Mat3) Add(m2 *Mat3) {
-
-	m.Data[0] += m2.Data[0]
-	m.Data[1] += m2.Data[1]
-	m.Data[2] += m2.Data[2]
-
-	m.Data[3] += m2.Data[3]
-	m.Data[4] += m2.Data[4]
-	m.Data[5] += m2.Data[5]
-
-	m.Data[6] += m2.Data[6]
-	m.Data[7] += m2.Data[7]
-	m.Data[8] += m2.Data[8]
+func (m *Mat3) Col(c int) *Vec3 {
+	return &Vec3{Data: m.Data[c]}
 }
 
-//Add m -= m2
-func (m *Mat3) Sub(m2 *Mat3) {
+// Add m += m2
+func (m *Mat3) Add(m2 *Mat3) *Mat3 {
 
-	m.Data[0] -= m2.Data[0]
-	m.Data[1] -= m2.Data[1]
-	m.Data[2] -= m2.Data[2]
+	m.Data[0][0] += m2.Data[0][0]
+	m.Data[0][1] += m2.Data[0][1]
+	m.Data[0][2] += m2.Data[0][2]
 
-	m.Data[3] -= m2.Data[3]
-	m.Data[4] -= m2.Data[4]
-	m.Data[5] -= m2.Data[5]
+	m.Data[1][0] += m2.Data[1][0]
+	m.Data[1][1] += m2.Data[1][1]
+	m.Data[1][2] += m2.Data[1][2]
 
-	m.Data[6] -= m2.Data[6]
-	m.Data[7] -= m2.Data[7]
-	m.Data[8] -= m2.Data[8]
+	m.Data[2][0] += m2.Data[2][0]
+	m.Data[2][1] += m2.Data[2][1]
+	m.Data[2][2] += m2.Data[2][2]
+	return m
 }
 
-//Mul m *= m2
-func (m *Mat3) Mul(m2 *Mat3) {
+// Add m -= m2
+func (m *Mat3) Sub(m2 *Mat3) *Mat3 {
 
-	//Indices:
-	// 0, 1, 2,
-	// 3, 4, 5,
-	// 6, 7, 8,
+	m.Data[0][0] -= m2.Data[0][0]
+	m.Data[0][1] -= m2.Data[0][1]
+	m.Data[0][2] -= m2.Data[0][2]
 
-	m.Data = [9]float32{
-		m.Data[0]*m2.Data[0] + m.Data[1]*m2.Data[3] + m.Data[2]*m2.Data[6],
-		m.Data[0]*m2.Data[1] + m.Data[1]*m2.Data[4] + m.Data[2]*m2.Data[7],
-		m.Data[0]*m2.Data[2] + m.Data[1]*m2.Data[5] + m.Data[2]*m2.Data[8],
+	m.Data[1][0] -= m2.Data[1][0]
+	m.Data[1][1] -= m2.Data[1][1]
+	m.Data[1][2] -= m2.Data[1][2]
 
-		m.Data[3]*m2.Data[0] + m.Data[4]*m2.Data[3] + m.Data[5]*m2.Data[6],
-		m.Data[3]*m2.Data[1] + m.Data[4]*m2.Data[4] + m.Data[5]*m2.Data[7],
-		m.Data[3]*m2.Data[2] + m.Data[4]*m2.Data[5] + m.Data[5]*m2.Data[8],
+	m.Data[2][0] -= m2.Data[2][0]
+	m.Data[2][1] -= m2.Data[2][1]
+	m.Data[2][2] -= m2.Data[2][2]
+	return m
+}
 
-		m.Data[6]*m2.Data[0] + m.Data[7]*m2.Data[3] + m.Data[8]*m2.Data[6],
-		m.Data[6]*m2.Data[1] + m.Data[7]*m2.Data[4] + m.Data[8]*m2.Data[7],
-		m.Data[6]*m2.Data[2] + m.Data[7]*m2.Data[5] + m.Data[8]*m2.Data[8],
+// Mul m *= m2
+func (m *Mat3) Mul(m2 *Mat3) *Mat3 {
+
+	//Array indices:
+	// 00, 10, 20,
+	// 01, 11, 21,
+	// 02, 12, 22,
+
+	m00 := m.Data[0][0]
+	m01 := m.Data[0][1]
+	m02 := m.Data[0][2]
+
+	m10 := m.Data[1][0]
+	m11 := m.Data[1][1]
+	m12 := m.Data[1][2]
+
+	m20 := m.Data[2][0]
+	m21 := m.Data[2][1]
+	m22 := m.Data[2][2]
+
+	m.Data = [3][3]float32{
+		{
+			m00*m2.Data[0][0] + m10*m2.Data[0][1] + m20*m2.Data[0][2],
+			m01*m2.Data[0][0] + m11*m2.Data[0][1] + m21*m2.Data[0][2],
+			m02*m2.Data[0][0] + m12*m2.Data[0][1] + m22*m2.Data[0][2],
+		},
+		{
+			m00*m2.Data[1][0] + m10*m2.Data[1][1] + m20*m2.Data[1][2],
+			m01*m2.Data[1][0] + m11*m2.Data[1][1] + m21*m2.Data[1][2],
+			m02*m2.Data[1][0] + m12*m2.Data[1][1] + m22*m2.Data[1][2],
+		},
+		{
+			m00*m2.Data[2][0] + m10*m2.Data[2][1] + m20*m2.Data[2][2],
+			m01*m2.Data[2][0] + m11*m2.Data[2][1] + m21*m2.Data[2][2],
+			m02*m2.Data[2][0] + m12*m2.Data[2][1] + m22*m2.Data[2][2],
+		},
+	}
+	return m
+}
+
+// Scale m *= x (element wise multiplication)
+func (m *Mat3) Scale(x float32) *Mat3 {
+
+	m.Data[0][0] *= x
+	m.Data[0][1] *= x
+	m.Data[0][2] *= x
+
+	m.Data[1][0] *= x
+	m.Data[1][1] *= x
+	m.Data[1][2] *= x
+
+	m.Data[2][0] *= x
+	m.Data[2][1] *= x
+	m.Data[2][2] *= x
+	return m
+}
+
+func (m *Mat3) Clone() *Mat3 {
+	return &Mat3{Data: m.Data}
+}
+
+func (m *Mat3) Eq(m2 *Mat3) bool {
+	return m.Data == m2.Data
+}
+
+func (m *Mat3) Transpose() *Mat3 {
+
+	m.Data = [3][3]float32{
+		{m.Data[0][0], m.Data[1][0], m.Data[2][0]},
+		{m.Data[0][1], m.Data[1][1], m.Data[2][1]},
+		{m.Data[0][2], m.Data[1][2], m.Data[2][2]},
+	}
+
+	return m
+}
+
+func (m *Mat3) Determinant() float32 {
+	x := m.Data[0][0] * (m.Data[1][1]*m.Data[2][2] - m.Data[2][1]*m.Data[1][2])
+	y := m.Data[1][0] * (m.Data[2][1]*m.Data[0][2] - m.Data[0][1]*m.Data[2][2])
+	z := m.Data[2][0] * (m.Data[0][1]*m.Data[1][2] - m.Data[1][1]*m.Data[0][2])
+	return x + y + z
+}
+
+// Invert inverts this matrix.
+//
+// Note that the inverse is not defined if the determinant is zero or extremely small.
+// In the case the determinant is zero the matrix will (usually) get filled with infinities
+func (m *Mat3) Invert() *Mat3 {
+
+	// https://www.cuemath.com/algebra/inverse-of-3x3-matrix/
+
+	// Manually inline the determinant function because go doesn't
+	x := m.Data[0][0] * (m.Data[1][1]*m.Data[2][2] - m.Data[2][1]*m.Data[1][2])
+	y := m.Data[1][0] * (m.Data[2][1]*m.Data[0][2] - m.Data[0][1]*m.Data[2][2])
+	z := m.Data[2][0] * (m.Data[0][1]*m.Data[1][2] - m.Data[1][1]*m.Data[0][2])
+
+	inverseDet := 1 / (x + y + z)
+
+	m.Data = [3][3]float32{
+		// Col0
+		{
+			(m.Data[1][1]*m.Data[2][2] - m.Data[2][1]*m.Data[1][2]) * inverseDet,
+			-(m.Data[0][1]*m.Data[2][2] - m.Data[2][1]*m.Data[0][2]) * inverseDet,
+			(m.Data[0][1]*m.Data[1][2] - m.Data[1][1]*m.Data[0][2]) * inverseDet,
+		},
+
+		// Col1
+		{
+			-(m.Data[1][0]*m.Data[2][2] - m.Data[2][0]*m.Data[1][2]) * inverseDet,
+			(m.Data[0][0]*m.Data[2][2] - m.Data[2][0]*m.Data[0][2]) * inverseDet,
+			-(m.Data[0][0]*m.Data[1][2] - m.Data[1][0]*m.Data[0][2]) * inverseDet,
+		},
+
+		// Col2
+		{
+			(m.Data[1][0]*m.Data[2][1] - m.Data[2][0]*m.Data[1][1]) * inverseDet,
+			-(m.Data[0][0]*m.Data[2][1] - m.Data[2][0]*m.Data[0][1]) * inverseDet,
+			(m.Data[0][0]*m.Data[1][1] - m.Data[1][0]*m.Data[0][1]) * inverseDet,
+		},
+	}
+
+	return m
+}
+
+// ToMat2 returns a Mat2 that contains the top-left 2x2 section of the Mat3.
+// That is, column 2 and row 2 are dropped.
+func (m *Mat3) ToMat2() Mat2 {
+	return Mat2{
+		Data: [2][2]float32{
+			{m.Data[0][0], m.Data[0][1]},
+			{m.Data[1][0], m.Data[1][1]},
+		},
 	}
 }
 
-//Scale m *= x (element wise multiplication)
-func (m *Mat3) Scale(x float32) {
-
-	m.Data[0] *= x
-	m.Data[1] *= x
-	m.Data[2] *= x
-
-	m.Data[3] *= x
-	m.Data[4] *= x
-	m.Data[5] *= x
-
-	m.Data[6] *= x
-	m.Data[7] *= x
-	m.Data[8] *= x
-}
-
-//AddMat3 m3 = m1 + m2
+// AddMat3 m3 = m1 + m2
 func AddMat3(m1, m2 *Mat3) *Mat3 {
 	return &Mat3{
-		Data: [9]float32{
-			m1.Data[0] + m2.Data[0],
-			m1.Data[1] + m2.Data[1],
-			m1.Data[2] + m2.Data[2],
-
-			m1.Data[3] + m2.Data[3],
-			m1.Data[4] + m2.Data[4],
-			m1.Data[5] + m2.Data[5],
-
-			m1.Data[6] + m2.Data[6],
-			m1.Data[7] + m2.Data[7],
-			m1.Data[8] + m2.Data[8],
+		Data: [3][3]float32{
+			{
+				m1.Data[0][0] + m2.Data[0][0],
+				m1.Data[0][1] + m2.Data[0][1],
+				m1.Data[0][2] + m2.Data[0][2],
+			},
+			{
+				m1.Data[1][0] + m2.Data[1][0],
+				m1.Data[1][1] + m2.Data[1][1],
+				m1.Data[1][2] + m2.Data[1][2],
+			},
+			{
+				m1.Data[2][0] + m2.Data[2][0],
+				m1.Data[2][1] + m2.Data[2][1],
+				m1.Data[2][2] + m2.Data[2][2],
+			},
 		},
 	}
 }
 
-//SubMat3 m3 = m1 - m2
+// SubMat3 m3 = m1 - m2
 func SubMat3(m1, m2 *Mat3) *Mat3 {
 	return &Mat3{
-		Data: [9]float32{
-			m1.Data[0] - m2.Data[0],
-			m1.Data[1] - m2.Data[1],
-			m1.Data[2] - m2.Data[2],
-
-			m1.Data[3] - m2.Data[3],
-			m1.Data[4] - m2.Data[4],
-			m1.Data[5] - m2.Data[5],
-
-			m1.Data[6] - m2.Data[6],
-			m1.Data[7] - m2.Data[7],
-			m1.Data[8] - m2.Data[8],
+		Data: [3][3]float32{
+			{
+				m1.Data[0][0] - m2.Data[0][0],
+				m1.Data[0][1] - m2.Data[0][1],
+				m1.Data[0][2] - m2.Data[0][2],
+			},
+			{
+				m1.Data[1][0] - m2.Data[1][0],
+				m1.Data[1][1] - m2.Data[1][1],
+				m1.Data[1][2] - m2.Data[1][2],
+			},
+			{
+				m1.Data[2][0] - m2.Data[2][0],
+				m1.Data[2][1] - m2.Data[2][1],
+				m1.Data[2][2] - m2.Data[2][2],
+			},
 		},
 	}
 }
 
-//MulMat3 m3 = m1 * m2
+// MulMat3 m3 = m1 * m2
 func MulMat3(m1, m2 *Mat3) *Mat3 {
+
+	m00 := m1.Data[0][0]
+	m01 := m1.Data[0][1]
+	m02 := m1.Data[0][2]
+
+	m10 := m1.Data[1][0]
+	m11 := m1.Data[1][1]
+	m12 := m1.Data[1][2]
+
+	m20 := m1.Data[2][0]
+	m21 := m1.Data[2][1]
+	m22 := m1.Data[2][2]
+
 	return &Mat3{
-		Data: [9]float32{
-			m1.Data[0]*m2.Data[0] + m1.Data[1]*m2.Data[3] + m1.Data[2]*m2.Data[6],
-			m1.Data[0]*m2.Data[1] + m1.Data[1]*m2.Data[4] + m1.Data[2]*m2.Data[7],
-			m1.Data[0]*m2.Data[2] + m1.Data[1]*m2.Data[5] + m1.Data[2]*m2.Data[8],
-
-			m1.Data[3]*m2.Data[0] + m1.Data[4]*m2.Data[3] + m1.Data[5]*m2.Data[6],
-			m1.Data[3]*m2.Data[1] + m1.Data[4]*m2.Data[4] + m1.Data[5]*m2.Data[7],
-			m1.Data[3]*m2.Data[2] + m1.Data[4]*m2.Data[5] + m1.Data[5]*m2.Data[8],
-
-			m1.Data[6]*m2.Data[0] + m1.Data[7]*m2.Data[3] + m1.Data[8]*m2.Data[6],
-			m1.Data[6]*m2.Data[1] + m1.Data[7]*m2.Data[4] + m1.Data[8]*m2.Data[7],
-			m1.Data[6]*m2.Data[2] + m1.Data[7]*m2.Data[5] + m1.Data[8]*m2.Data[8],
+		Data: [3][3]float32{
+			{
+				m00*m2.Data[0][0] + m10*m2.Data[0][1] + m20*m2.Data[0][2],
+				m01*m2.Data[0][0] + m11*m2.Data[0][1] + m21*m2.Data[0][2],
+				m02*m2.Data[0][0] + m12*m2.Data[0][1] + m22*m2.Data[0][2],
+			},
+			{
+				m00*m2.Data[1][0] + m10*m2.Data[1][1] + m20*m2.Data[1][2],
+				m01*m2.Data[1][0] + m11*m2.Data[1][1] + m21*m2.Data[1][2],
+				m02*m2.Data[1][0] + m12*m2.Data[1][1] + m22*m2.Data[1][2],
+			},
+			{
+				m00*m2.Data[2][0] + m10*m2.Data[2][1] + m20*m2.Data[2][2],
+				m01*m2.Data[2][0] + m11*m2.Data[2][1] + m21*m2.Data[2][2],
+				m02*m2.Data[2][0] + m12*m2.Data[2][1] + m22*m2.Data[2][2],
+			},
 		},
 	}
 }
 
-//NewMat3Id returns the 3x3 identity matrix
+// MulMat3Vec3 v2 = m1 * v1
+func MulMat3Vec3(m1 *Mat3, v1 *Vec3) *Vec3 {
+	return &Vec3{
+		Data: [3]float32{
+			m1.Data[0][0]*v1.Data[0] + m1.Data[1][0]*v1.Data[1] + m1.Data[2][0]*v1.Data[2],
+			m1.Data[0][1]*v1.Data[0] + m1.Data[1][1]*v1.Data[1] + m1.Data[2][1]*v1.Data[2],
+			m1.Data[0][2]*v1.Data[0] + m1.Data[1][2]*v1.Data[1] + m1.Data[2][2]*v1.Data[2],
+		},
+	}
+}
+
+// NewMat3Id returns the 3x3 identity matrix
 func NewMat3Id() *Mat3 {
 	return &Mat3{
-		Data: [9]float32{
-			1, 0, 0,
-			0, 1, 0,
-			0, 0, 1,
+		Data: [3][3]float32{
+			{1, 0, 0},
+			{0, 1, 0},
+			{0, 0, 1},
+		},
+	}
+}
+
+func NewMat3Diag(diagVal float32) *Mat3 {
+	return &Mat3{
+		Data: [3][3]float32{
+			{diagVal, 0, 0},
+			{0, diagVal, 0},
+			{0, 0, diagVal},
+		},
+	}
+}
+
+func NewMat3DiagArr(diag [3]float32) *Mat3 {
+	return &Mat3{
+		Data: [3][3]float32{
+			{diag[0], 0, 0},
+			{0, diag[1], 0},
+			{0, 0, diag[2]},
+		},
+	}
+}
+
+func NewMat3Vec3(col0, col1, col2 *Vec3) *Mat3 {
+	return &Mat3{
+		Data: [3][3]float32{
+			col0.Data,
+			col1.Data,
+			col2.Data,
+		},
+	}
+}
+
+func NewMat3Arr(col0, col1, col2 [3]float32) *Mat3 {
+	return &Mat3{
+		Data: [3][3]float32{
+			col0,
+			col1,
+			col2,
 		},
 	}
 }

gglm/mat3_test.go

@@ -0,0 +1,260 @@
+package gglm_test
+
+import (
+	"testing"
+
+	"github.com/bloeys/gglm/gglm"
+)
+
+func TestMat3GetSet(t *testing.T) {
+
+	m1 := gglm.Mat3{}
+
+	m1.Set(0, 1, -10)
+	m1.Set(1, 0, 55)
+	m1.Set(2, 2, 99)
+
+	if m1.Get(0, 1) != -10 {
+		t.Errorf("Got: %v; Expected: %v", m1.Get(0, 1), -10)
+	}
+
+	if m1.Get(1, 0) != 55 {
+		t.Errorf("Got: %v; Expected: %v", m1.Get(1, 0), 55)
+	}
+
+	if m1.Get(2, 2) != 99 {
+		t.Errorf("Got: %v; Expected: %v", m1.Get(2, 2), 99)
+	}
+}
+
+func TestMat3Id(t *testing.T) {
+
+	correctAns := gglm.Mat3{
+		Data: [3][3]float32{
+			{1, 0, 0},
+			{0, 1, 0},
+			{0, 0, 1},
+		}}
+
+	m1 := gglm.NewMat3Id()
+	if !m1.Eq(&correctAns) {
+		t.Errorf("Got: %v; Expected: %v", m1.String(), correctAns.String())
+	}
+}
+
+func TestSubMat3(t *testing.T) {
+
+	correctAns := gglm.Mat3{
+		Data: [3][3]float32{
+			{-9, -9, -9},
+			{-9, -9, -9},
+			{-9, -9, -9},
+		}}
+
+	m1 := &gglm.Mat3{
+		Data: [3][3]float32{
+			{1, 4, 7},
+			{2, 5, 8},
+			{3, 6, 9},
+		}}
+	m2 := &gglm.Mat3{
+		Data: [3][3]float32{
+			{10, 13, 16},
+			{11, 14, 17},
+			{12, 15, 18},
+		}}
+
+	result := gglm.SubMat3(m1, m2)
+	m1.Sub(m2)
+
+	if !result.Eq(&correctAns) {
+		t.Errorf("Got: %v; Expected: %v", result.String(), correctAns.String())
+	}
+
+	if !m1.Eq(&correctAns) {
+		t.Errorf("Got: %v; Expected: %v", m1.String(), correctAns.String())
+	}
+}
+
+func TestAddMat3(t *testing.T) {
+
+	correctAns := gglm.Mat3{
+		Data: [3][3]float32{
+			{11, 17, 23},
+			{13, 19, 25},
+			{15, 21, 27},
+		}}
+
+	m1 := &gglm.Mat3{
+		Data: [3][3]float32{
+			{1, 4, 7},
+			{2, 5, 8},
+			{3, 6, 9},
+		}}
+	m2 := &gglm.Mat3{
+		Data: [3][3]float32{
+			{10, 13, 16},
+			{11, 14, 17},
+			{12, 15, 18},
+		}}
+
+	result := gglm.AddMat3(m1, m2)
+	m1.Add(m2)
+
+	if !result.Eq(&correctAns) {
+		t.Errorf("Got: %v; Expected: %v", result.String(), correctAns.String())
+	}
+
+	if !m1.Eq(&correctAns) {
+		t.Errorf("Got: %v; Expected: %v", m1.String(), correctAns.String())
+	}
+}
+
+func TestMulMat3(t *testing.T) {
+
+	correctAns := gglm.Mat3{
+		Data: [3][3]float32{
+			{84, 201, 318},
+			{90, 216, 342},
+			{96, 231, 366},
+		}}
+
+	m1 := &gglm.Mat3{
+		Data: [3][3]float32{
+			{1, 4, 7},
+			{2, 5, 8},
+			{3, 6, 9},
+		}}
+	m2 := &gglm.Mat3{
+		Data: [3][3]float32{
+			{10, 13, 16},
+			{11, 14, 17},
+			{12, 15, 18},
+		}}
+
+	result := gglm.MulMat3(m1, m2)
+	m1.Mul(m2)
+
+	if !result.Eq(&correctAns) {
+		t.Errorf("Got: %v; Expected: %v", result.String(), correctAns.String())
+	}
+
+	if !m1.Eq(&correctAns) {
+		t.Errorf("Got: %v; Expected: %v", m1.String(), correctAns.String())
+	}
+}
+
+func TestMulMat3Vec3(t *testing.T) {
+
+	m := &gglm.Mat3{
+		Data: [3][3]float32{
+			{1, 4, 7},
+			{2, 5, 8},
+			{3, 6, 9},
+		}}
+	v := &gglm.Vec3{Data: [3]float32{1, 2, 3}}
+
+	result := gglm.MulMat3Vec3(m, v)
+	correctAns := gglm.Vec3{Data: [3]float32{14, 32, 50}}
+
+	if !result.Eq(&correctAns) {
+		t.Errorf("Got: %v; Expected: %v", result.String(), correctAns.String())
+	}
+}
+
+func TestTransposeMat3(t *testing.T) {
+
+	m := gglm.NewMat3Id()
+	ans := gglm.NewMat3Id()
+
+	if !m.Transpose().Transpose().Eq(ans) {
+		t.Errorf("Got: %v; Expected: %v", m.String(), ans.String())
+	}
+
+	if !m.Transpose().Eq(ans) {
+		t.Errorf("Got: %v; Expected: %v", m.String(), ans.String())
+	}
+
+	m = &gglm.Mat3{
+		Data: [3][3]float32{
+			{00, 01, 02},
+			{10, 11, 12},
+			{20, 21, 22},
+		},
+	}
+
+	ans = &gglm.Mat3{
+		Data: [3][3]float32{
+			{00, 10, 20},
+			{01, 11, 21},
+			{02, 12, 22},
+		},
+	}
+
+	if !m.Transpose().Eq(ans) {
+		t.Errorf("Got: %v; Expected: %v", m.String(), ans.String())
+	}
+}
+
+func TestDeterminantMat3(t *testing.T) {
+
+	m := gglm.NewMat3Id()
+	ans := float32(1)
+
+	if m.Determinant() != ans {
+		t.Errorf("Got: %f; Expected: %f", m.Determinant(), ans)
+	}
+
+	m = &gglm.Mat3{
+		Data: [3][3]float32{
+			{1, 8, 2},
+			{5, 3, 5},
+			{9, 6, 7},
+		},
+	}
+	ans = 77
+
+	if m.Determinant() != ans {
+		t.Errorf("Got: %f; Expected: %f", m.Determinant(), ans)
+	}
+}
+
+func TestInvertMat3(t *testing.T) {
+
+	m := gglm.NewMat3Id()
+	ans := gglm.NewMat3Id()
+
+	if !m.Invert().Eq(ans) {
+		t.Errorf("Got: %v; Expected: %v", m.String(), ans.String())
+	}
+
+	m = &gglm.Mat3{
+		Data: [3][3]float32{
+			{1, 8, 2},
+			{5, 3, 5},
+			{9, 6, 7},
+		},
+	}
+
+	ans = &gglm.Mat3{
+		Data: [3][3]float32{
+			{-9 / 77.0, -4 / 7.0, 34 / 77.0},
+			{10 / 77.0, -1 / 7.0, 5 / 77.0},
+			{3 / 77.0, 6 / 7.0, -37 / 77.0},
+		},
+	}
+
+	if !m.Invert().Eq(ans) {
+		t.Errorf("Got: %v; Expected: %v", m.String(), ans.String())
+	}
+}
+
+func BenchmarkMulMat3(b *testing.B) {
+
+	m1 := gglm.NewMat3Id()
+	m2 := gglm.NewMat3Id()
+
+	for i := 0; i < b.N; i++ {
+		m1.Mul(m2)
+	}
+}

gglm/mat4.go

@@ -8,15 +8,15 @@ var _ Mat = &Mat4{}
 var _ fmt.Stringer = &Mat4{}
 
 type Mat4 struct {
-	Data [16]float32
+	Data [4][4]float32
 }
 
-func (m *Mat4) At(row, col int) float32 {
-	return m.Data[row*4+col]
+func (m *Mat4) Get(row, col int) float32 {
+	return m.Data[col][row]
 }
 
 func (m *Mat4) Set(row, col int, val float32) {
-	m.Data[row*4+col] = val
+	m.Data[col][row] = val
 }
 
 func (m *Mat4) Size() MatSize {
@@ -25,206 +25,656 @@ func (m *Mat4) Size() MatSize {
 
 func (m *Mat4) String() string {
 	return fmt.Sprintf("\n| %+-9.3f %+-9.3f %+-9.3f %+-9.3f |\n| %+-9.3f %+-9.3f %+-9.3f %+-9.3f |\n| %+-9.3f %+-9.3f %+-9.3f %+-9.3f |\n| %+-9.3f %+-9.3f %+-9.3f %+-9.3f |\n",
-		m.Data[0], m.Data[1], m.Data[2], m.Data[3],
-		m.Data[4], m.Data[5], m.Data[6], m.Data[7],
-		m.Data[8], m.Data[9], m.Data[10], m.Data[11],
-		m.Data[12], m.Data[13], m.Data[14], m.Data[15],
+		m.Data[0][0], m.Data[0][1], m.Data[0][2], m.Data[0][3],
+		m.Data[1][0], m.Data[1][1], m.Data[1][2], m.Data[1][3],
+		m.Data[2][0], m.Data[2][1], m.Data[2][2], m.Data[2][3],
+		m.Data[3][0], m.Data[3][1], m.Data[3][2], m.Data[3][3],
 	)
 }
 
-//Add m += m2
-func (m *Mat4) Add(m2 *Mat4) {
-
-	m.Data[0] += m2.Data[0]
-	m.Data[1] += m2.Data[1]
-	m.Data[2] += m2.Data[2]
-	m.Data[3] += m2.Data[3]
-
-	m.Data[4] += m2.Data[4]
-	m.Data[5] += m2.Data[5]
-	m.Data[6] += m2.Data[6]
-	m.Data[7] += m2.Data[7]
-
-	m.Data[8] += m2.Data[8]
-	m.Data[9] += m2.Data[9]
-	m.Data[10] += m2.Data[10]
-	m.Data[11] += m2.Data[11]
-
-	m.Data[12] += m2.Data[12]
-	m.Data[13] += m2.Data[13]
-	m.Data[14] += m2.Data[14]
-	m.Data[15] += m2.Data[15]
+func (m *Mat4) Col(c int) *Vec4 {
+	return &Vec4{Data: m.Data[c]}
 }
 
-//Add m -= m2
-func (m *Mat4) Sub(m2 *Mat4) {
+// Add m += m2
+func (m *Mat4) Add(m2 *Mat4) *Mat4 {
 
-	m.Data[0] -= m2.Data[0]
-	m.Data[1] -= m2.Data[1]
-	m.Data[2] -= m2.Data[2]
-	m.Data[3] -= m2.Data[3]
+	m.Data[0][0] += m2.Data[0][0]
+	m.Data[0][1] += m2.Data[0][1]
+	m.Data[0][2] += m2.Data[0][2]
+	m.Data[0][3] += m2.Data[0][3]
 
-	m.Data[4] -= m2.Data[4]
-	m.Data[5] -= m2.Data[5]
-	m.Data[6] -= m2.Data[6]
-	m.Data[7] -= m2.Data[7]
+	m.Data[1][0] += m2.Data[1][0]
+	m.Data[1][1] += m2.Data[1][1]
+	m.Data[1][2] += m2.Data[1][2]
+	m.Data[1][3] += m2.Data[1][3]
 
-	m.Data[8] -= m2.Data[8]
-	m.Data[9] -= m2.Data[9]
-	m.Data[10] -= m2.Data[10]
-	m.Data[11] -= m2.Data[11]
+	m.Data[2][0] += m2.Data[2][0]
+	m.Data[2][1] += m2.Data[2][1]
+	m.Data[2][2] += m2.Data[2][2]
+	m.Data[2][3] += m2.Data[2][3]
 
-	m.Data[12] -= m2.Data[12]
-	m.Data[13] -= m2.Data[13]
-	m.Data[14] -= m2.Data[14]
-	m.Data[15] -= m2.Data[15]
+	m.Data[3][0] += m2.Data[3][0]
+	m.Data[3][1] += m2.Data[3][1]
+	m.Data[3][2] += m2.Data[3][2]
+	m.Data[3][3] += m2.Data[3][3]
+
+	return m
 }
 
-//Mul m *= m2
-func (m *Mat4) Mul(m2 *Mat4) {
+// Add m -= m2
+func (m *Mat4) Sub(m2 *Mat4) *Mat4 {
 
-	//Indices:
-	// 00, 01, 02, 03,
-	// 04, 05, 06, 07,
-	// 08, 09, 10, 11,
-	// 12, 13, 14, 15,
+	m.Data[0][0] -= m2.Data[0][0]
+	m.Data[0][1] -= m2.Data[0][1]
+	m.Data[0][2] -= m2.Data[0][2]
+	m.Data[0][3] -= m2.Data[0][3]
 
-	m.Data = [16]float32{
-		m.Data[0]*m2.Data[0] + m.Data[1]*m2.Data[4] + m.Data[2]*m2.Data[8] + m.Data[3]*m2.Data[12],
-		m.Data[0]*m2.Data[1] + m.Data[1]*m2.Data[5] + m.Data[2]*m2.Data[9] + m.Data[3]*m2.Data[13],
-		m.Data[0]*m2.Data[2] + m.Data[1]*m2.Data[6] + m.Data[2]*m2.Data[10] + m.Data[3]*m2.Data[14],
-		m.Data[0]*m2.Data[3] + m.Data[1]*m2.Data[7] + m.Data[2]*m2.Data[11] + m.Data[3]*m2.Data[15],
+	m.Data[1][0] -= m2.Data[1][0]
+	m.Data[1][1] -= m2.Data[1][1]
+	m.Data[1][2] -= m2.Data[1][2]
+	m.Data[1][3] -= m2.Data[1][3]
 
-		m.Data[4]*m2.Data[0] + m.Data[5]*m2.Data[4] + m.Data[6]*m2.Data[8] + m.Data[7]*m2.Data[12],
-		m.Data[4]*m2.Data[1] + m.Data[5]*m2.Data[5] + m.Data[6]*m2.Data[9] + m.Data[7]*m2.Data[13],
-		m.Data[4]*m2.Data[2] + m.Data[5]*m2.Data[6] + m.Data[6]*m2.Data[10] + m.Data[7]*m2.Data[14],
-		m.Data[4]*m2.Data[3] + m.Data[5]*m2.Data[7] + m.Data[6]*m2.Data[11] + m.Data[7]*m2.Data[15],
+	m.Data[2][0] -= m2.Data[2][0]
+	m.Data[2][1] -= m2.Data[2][1]
+	m.Data[2][2] -= m2.Data[2][2]
+	m.Data[2][3] -= m2.Data[2][3]
 
-		m.Data[8]*m2.Data[0] + m.Data[9]*m2.Data[4] + m.Data[10]*m2.Data[8] + m.Data[11]*m2.Data[12],
-		m.Data[8]*m2.Data[1] + m.Data[9]*m2.Data[5] + m.Data[10]*m2.Data[9] + m.Data[11]*m2.Data[13],
-		m.Data[8]*m2.Data[2] + m.Data[9]*m2.Data[6] + m.Data[10]*m2.Data[10] + m.Data[11]*m2.Data[14],
-		m.Data[8]*m2.Data[3] + m.Data[9]*m2.Data[7] + m.Data[10]*m2.Data[11] + m.Data[11]*m2.Data[15],
+	m.Data[3][0] -= m2.Data[3][0]
+	m.Data[3][1] -= m2.Data[3][1]
+	m.Data[3][2] -= m2.Data[3][2]
+	m.Data[3][3] -= m2.Data[3][3]
+	return m
+}
 
-		m.Data[12]*m2.Data[0] + m.Data[13]*m2.Data[4] + m.Data[14]*m2.Data[8] + m.Data[15]*m2.Data[12],
-		m.Data[12]*m2.Data[1] + m.Data[13]*m2.Data[5] + m.Data[14]*m2.Data[9] + m.Data[15]*m2.Data[13],
-		m.Data[12]*m2.Data[2] + m.Data[13]*m2.Data[6] + m.Data[14]*m2.Data[10] + m.Data[15]*m2.Data[14],
-		m.Data[12]*m2.Data[3] + m.Data[13]*m2.Data[7] + m.Data[14]*m2.Data[11] + m.Data[15]*m2.Data[15],
+// Mul m *= m2
+func (m *Mat4) Mul(m2 *Mat4) *Mat4 {
+
+	//Array indices:
+	// 00, 10, 20, 30,
+	// 01, 11, 21, 31,
+	// 02, 12, 22, 32,
+	// 03, 13, 23, 33,
+
+	//Improves performance by ~8%
+	m00 := m.Data[0][0]
+	m01 := m.Data[0][1]
+	m02 := m.Data[0][2]
+	m03 := m.Data[0][3]
+
+	m10 := m.Data[1][0]
+	m11 := m.Data[1][1]
+	m12 := m.Data[1][2]
+	m13 := m.Data[1][3]
+
+	m20 := m.Data[2][0]
+	m21 := m.Data[2][1]
+	m22 := m.Data[2][2]
+	m23 := m.Data[2][3]
+
+	m30 := m.Data[3][0]
+	m31 := m.Data[3][1]
+	m32 := m.Data[3][2]
+	m33 := m.Data[3][3]
+
+	m.Data = [4][4]float32{
+		{
+			m00*m2.Data[0][0] + m10*m2.Data[0][1] + m20*m2.Data[0][2] + m30*m2.Data[0][3],
+			m01*m2.Data[0][0] + m11*m2.Data[0][1] + m21*m2.Data[0][2] + m31*m2.Data[0][3],
+			m02*m2.Data[0][0] + m12*m2.Data[0][1] + m22*m2.Data[0][2] + m32*m2.Data[0][3],
+			m03*m2.Data[0][0] + m13*m2.Data[0][1] + m23*m2.Data[0][2] + m33*m2.Data[0][3],
+		},
+		{
+			m00*m2.Data[1][0] + m10*m2.Data[1][1] + m20*m2.Data[1][2] + m30*m2.Data[1][3],
+			m01*m2.Data[1][0] + m11*m2.Data[1][1] + m21*m2.Data[1][2] + m31*m2.Data[1][3],
+			m02*m2.Data[1][0] + m12*m2.Data[1][1] + m22*m2.Data[1][2] + m32*m2.Data[1][3],
+			m03*m2.Data[1][0] + m13*m2.Data[1][1] + m23*m2.Data[1][2] + m33*m2.Data[1][3],
+		},
+		{
+			m00*m2.Data[2][0] + m10*m2.Data[2][1] + m20*m2.Data[2][2] + m30*m2.Data[2][3],
+			m01*m2.Data[2][0] + m11*m2.Data[2][1] + m21*m2.Data[2][2] + m31*m2.Data[2][3],
+			m02*m2.Data[2][0] + m12*m2.Data[2][1] + m22*m2.Data[2][2] + m32*m2.Data[2][3],
+			m03*m2.Data[2][0] + m13*m2.Data[2][1] + m23*m2.Data[2][2] + m33*m2.Data[2][3],
+		},
+		{
+			m00*m2.Data[3][0] + m10*m2.Data[3][1] + m20*m2.Data[3][2] + m30*m2.Data[3][3],
+			m01*m2.Data[3][0] + m11*m2.Data[3][1] + m21*m2.Data[3][2] + m31*m2.Data[3][3],
+			m02*m2.Data[3][0] + m12*m2.Data[3][1] + m22*m2.Data[3][2] + m32*m2.Data[3][3],
+			m03*m2.Data[3][0] + m13*m2.Data[3][1] + m23*m2.Data[3][2] + m33*m2.Data[3][3],
+		},
+	}
+
+	return m
+}
+
+// Scale m *= x (element wise multiplication)
+func (m *Mat4) Scale(x float32) *Mat4 {
+
+	m.Data[0][0] *= x
+	m.Data[0][1] *= x
+	m.Data[0][2] *= x
+	m.Data[0][3] *= x
+
+	m.Data[1][0] *= x
+	m.Data[1][1] *= x
+	m.Data[1][2] *= x
+	m.Data[1][3] *= x
+
+	m.Data[2][0] *= x
+	m.Data[2][1] *= x
+	m.Data[2][2] *= x
+	m.Data[2][3] *= x
+
+	m.Data[3][0] *= x
+	m.Data[3][1] *= x
+	m.Data[3][2] *= x
+	m.Data[3][3] *= x
+	return m
+}
+
+func (v *Mat4) Clone() *Mat4 {
+	return &Mat4{Data: v.Data}
+}
+
+func (m *Mat4) Eq(m2 *Mat4) bool {
+	return m.Data == m2.Data
+}
+
+func (m *Mat4) Transpose() *Mat4 {
+
+	m.Data = [4][4]float32{
+		{m.Data[0][0], m.Data[1][0], m.Data[2][0], m.Data[3][0]},
+		{m.Data[0][1], m.Data[1][1], m.Data[2][1], m.Data[3][1]},
+		{m.Data[0][2], m.Data[1][2], m.Data[2][2], m.Data[3][2]},
+		{m.Data[0][3], m.Data[1][3], m.Data[2][3], m.Data[3][3]},
+	}
+
+	return m
+}
+
+func (m *Mat4) Determinant() float32 {
+
+	// Many thanks to the C++ GLM project here :)
+
+	coef00 := m.Data[2][2]*m.Data[3][3] - m.Data[3][2]*m.Data[2][3]
+	coef02 := m.Data[1][2]*m.Data[3][3] - m.Data[3][2]*m.Data[1][3]
+	coef03 := m.Data[1][2]*m.Data[2][3] - m.Data[2][2]*m.Data[1][3]
+
+	coef04 := m.Data[2][1]*m.Data[3][3] - m.Data[3][1]*m.Data[2][3]
+	coef06 := m.Data[1][1]*m.Data[3][3] - m.Data[3][1]*m.Data[1][3]
+	coef07 := m.Data[1][1]*m.Data[2][3] - m.Data[2][1]*m.Data[1][3]
+
+	coef08 := m.Data[2][1]*m.Data[3][2] - m.Data[3][1]*m.Data[2][2]
+	coef10 := m.Data[1][1]*m.Data[3][2] - m.Data[3][1]*m.Data[1][2]
+	coef11 := m.Data[1][1]*m.Data[2][2] - m.Data[2][1]*m.Data[1][2]
+
+	coef12 := m.Data[2][0]*m.Data[3][3] - m.Data[3][0]*m.Data[2][3]
+	coef14 := m.Data[1][0]*m.Data[3][3] - m.Data[3][0]*m.Data[1][3]
+	coef15 := m.Data[1][0]*m.Data[2][3] - m.Data[2][0]*m.Data[1][3]
+
+	coef16 := m.Data[2][0]*m.Data[3][2] - m.Data[3][0]*m.Data[2][2]
+	coef18 := m.Data[1][0]*m.Data[3][2] - m.Data[3][0]*m.Data[1][2]
+	coef19 := m.Data[1][0]*m.Data[2][2] - m.Data[2][0]*m.Data[1][2]
+
+	coef20 := m.Data[2][0]*m.Data[3][1] - m.Data[3][0]*m.Data[2][1]
+	coef22 := m.Data[1][0]*m.Data[3][1] - m.Data[3][0]*m.Data[1][1]
+	coef23 := m.Data[1][0]*m.Data[2][1] - m.Data[2][0]*m.Data[1][1]
+
+	fac0 := NewVec4(coef00, coef00, coef02, coef03)
+	fac1 := NewVec4(coef04, coef04, coef06, coef07)
+	fac2 := NewVec4(coef08, coef08, coef10, coef11)
+	fac3 := NewVec4(coef12, coef12, coef14, coef15)
+	fac4 := NewVec4(coef16, coef16, coef18, coef19)
+	fac5 := NewVec4(coef20, coef20, coef22, coef23)
+
+	vec0 := NewVec4(m.Data[1][0], m.Data[0][0], m.Data[0][0], m.Data[0][0])
+	vec1 := NewVec4(m.Data[1][1], m.Data[0][1], m.Data[0][1], m.Data[0][1])
+	vec2 := NewVec4(m.Data[1][2], m.Data[0][2], m.Data[0][2], m.Data[0][2])
+	vec3 := NewVec4(m.Data[1][3], m.Data[0][3], m.Data[0][3], m.Data[0][3])
+
+	inv0 := NewVec4(
+		vec1.X()*fac0.X()-vec2.X()*fac1.X()+vec3.X()*fac2.X(),
+		vec1.Y()*fac0.Y()-vec2.Y()*fac1.Y()+vec3.Y()*fac2.Y(),
+		vec1.Z()*fac0.Z()-vec2.Z()*fac1.Z()+vec3.Z()*fac2.Z(),
+		vec1.W()*fac0.W()-vec2.W()*fac1.W()+vec3.W()*fac2.W(),
+	)
+
+	inv1 := NewVec4(
+		vec0.X()*fac0.X()-vec2.X()*fac3.X()+vec3.X()*fac4.X(),
+		vec0.Y()*fac0.Y()-vec2.Y()*fac3.Y()+vec3.Y()*fac4.Y(),
+		vec0.Z()*fac0.Z()-vec2.Z()*fac3.Z()+vec3.Z()*fac4.Z(),
+		vec0.W()*fac0.W()-vec2.W()*fac3.W()+vec3.W()*fac4.W(),
+	)
+
+	inv2 := NewVec4(
+		vec0.X()*fac1.X()-vec1.X()*fac3.X()+vec3.X()*fac5.X(),
+		vec0.Y()*fac1.Y()-vec1.Y()*fac3.Y()+vec3.Y()*fac5.Y(),
+		vec0.Z()*fac1.Z()-vec1.Z()*fac3.Z()+vec3.Z()*fac5.Z(),
+		vec0.W()*fac1.W()-vec1.W()*fac3.W()+vec3.W()*fac5.W(),
+	)
+
+	inv3 := NewVec4(
+		vec0.X()*fac2.X()-vec1.X()*fac4.X()+vec2.X()*fac5.X(),
+		vec0.Y()*fac2.Y()-vec1.Y()*fac4.Y()+vec2.Y()*fac5.Y(),
+		vec0.Z()*fac2.Z()-vec1.Z()*fac4.Z()+vec2.Z()*fac5.Z(),
+		vec0.W()*fac2.W()-vec1.W()*fac4.W()+vec2.W()*fac5.W(),
+	)
+
+	signA := NewVec4(+1, -1, +1, -1)
+	signB := NewVec4(-1, +1, -1, +1)
+
+	inverse := NewMat4Arr(
+		inv0.ScaleVec(signA).Data,
+		inv1.ScaleVec(signB).Data,
+		inv2.ScaleVec(signA).Data,
+		inv3.ScaleVec(signB).Data,
+	)
+
+	row0 := NewVec4(inverse.Data[0][0], inverse.Data[1][0], inverse.Data[2][0], inverse.Data[3][0])
+	dot0 := NewVec4Arr(row0.ScaleArr(m.Data[0]).Data)
+	det := (dot0.X() + dot0.Y()) + (dot0.Z() + dot0.W())
+
+	return det
+}
+
+// Invert inverts this matrix.
+//
+// Note that the inverse is not defined if the determinant is zero or extremely small.
+// In the case the determinant is zero the matrix will (usually) get filled with infinities
+func (m *Mat4) Invert() *Mat4 {
+
+	// Many thanks to the C++ GLM project here :)
+
+	coef00 := m.Data[2][2]*m.Data[3][3] - m.Data[3][2]*m.Data[2][3]
+	coef02 := m.Data[1][2]*m.Data[3][3] - m.Data[3][2]*m.Data[1][3]
+	coef03 := m.Data[1][2]*m.Data[2][3] - m.Data[2][2]*m.Data[1][3]
+
+	coef04 := m.Data[2][1]*m.Data[3][3] - m.Data[3][1]*m.Data[2][3]
+	coef06 := m.Data[1][1]*m.Data[3][3] - m.Data[3][1]*m.Data[1][3]
+	coef07 := m.Data[1][1]*m.Data[2][3] - m.Data[2][1]*m.Data[1][3]
+
+	coef08 := m.Data[2][1]*m.Data[3][2] - m.Data[3][1]*m.Data[2][2]
+	coef10 := m.Data[1][1]*m.Data[3][2] - m.Data[3][1]*m.Data[1][2]
+	coef11 := m.Data[1][1]*m.Data[2][2] - m.Data[2][1]*m.Data[1][2]
+
+	coef12 := m.Data[2][0]*m.Data[3][3] - m.Data[3][0]*m.Data[2][3]
+	coef14 := m.Data[1][0]*m.Data[3][3] - m.Data[3][0]*m.Data[1][3]
+	coef15 := m.Data[1][0]*m.Data[2][3] - m.Data[2][0]*m.Data[1][3]
+
+	coef16 := m.Data[2][0]*m.Data[3][2] - m.Data[3][0]*m.Data[2][2]
+	coef18 := m.Data[1][0]*m.Data[3][2] - m.Data[3][0]*m.Data[1][2]
+	coef19 := m.Data[1][0]*m.Data[2][2] - m.Data[2][0]*m.Data[1][2]
+
+	coef20 := m.Data[2][0]*m.Data[3][1] - m.Data[3][0]*m.Data[2][1]
+	coef22 := m.Data[1][0]*m.Data[3][1] - m.Data[3][0]*m.Data[1][1]
+	coef23 := m.Data[1][0]*m.Data[2][1] - m.Data[2][0]*m.Data[1][1]
+
+	fac0 := NewVec4(coef00, coef00, coef02, coef03)
+	fac1 := NewVec4(coef04, coef04, coef06, coef07)
+	fac2 := NewVec4(coef08, coef08, coef10, coef11)
+	fac3 := NewVec4(coef12, coef12, coef14, coef15)
+	fac4 := NewVec4(coef16, coef16, coef18, coef19)
+	fac5 := NewVec4(coef20, coef20, coef22, coef23)
+
+	vec0 := NewVec4(m.Data[1][0], m.Data[0][0], m.Data[0][0], m.Data[0][0])
+	vec1 := NewVec4(m.Data[1][1], m.Data[0][1], m.Data[0][1], m.Data[0][1])
+	vec2 := NewVec4(m.Data[1][2], m.Data[0][2], m.Data[0][2], m.Data[0][2])
+	vec3 := NewVec4(m.Data[1][3], m.Data[0][3], m.Data[0][3], m.Data[0][3])
+
+	inv0 := NewVec4(
+		vec1.X()*fac0.X()-vec2.X()*fac1.X()+vec3.X()*fac2.X(),
+		vec1.Y()*fac0.Y()-vec2.Y()*fac1.Y()+vec3.Y()*fac2.Y(),
+		vec1.Z()*fac0.Z()-vec2.Z()*fac1.Z()+vec3.Z()*fac2.Z(),
+		vec1.W()*fac0.W()-vec2.W()*fac1.W()+vec3.W()*fac2.W(),
+	)
+
+	inv1 := NewVec4(
+		vec0.X()*fac0.X()-vec2.X()*fac3.X()+vec3.X()*fac4.X(),
+		vec0.Y()*fac0.Y()-vec2.Y()*fac3.Y()+vec3.Y()*fac4.Y(),
+		vec0.Z()*fac0.Z()-vec2.Z()*fac3.Z()+vec3.Z()*fac4.Z(),
+		vec0.W()*fac0.W()-vec2.W()*fac3.W()+vec3.W()*fac4.W(),
+	)
+
+	inv2 := NewVec4(
+		vec0.X()*fac1.X()-vec1.X()*fac3.X()+vec3.X()*fac5.X(),
+		vec0.Y()*fac1.Y()-vec1.Y()*fac3.Y()+vec3.Y()*fac5.Y(),
+		vec0.Z()*fac1.Z()-vec1.Z()*fac3.Z()+vec3.Z()*fac5.Z(),
+		vec0.W()*fac1.W()-vec1.W()*fac3.W()+vec3.W()*fac5.W(),
+	)
+
+	inv3 := NewVec4(
+		vec0.X()*fac2.X()-vec1.X()*fac4.X()+vec2.X()*fac5.X(),
+		vec0.Y()*fac2.Y()-vec1.Y()*fac4.Y()+vec2.Y()*fac5.Y(),
+		vec0.Z()*fac2.Z()-vec1.Z()*fac4.Z()+vec2.Z()*fac5.Z(),
+		vec0.W()*fac2.W()-vec1.W()*fac4.W()+vec2.W()*fac5.W(),
+	)
+
+	signA := NewVec4(+1, -1, +1, -1)
+	signB := NewVec4(-1, +1, -1, +1)
+
+	inverse := NewMat4Arr(
+		inv0.ScaleVec(signA).Data,
+		inv1.ScaleVec(signB).Data,
+		inv2.ScaleVec(signA).Data,
+		inv3.ScaleVec(signB).Data,
+	)
+
+	row0 := NewVec4(inverse.Data[0][0], inverse.Data[1][0], inverse.Data[2][0], inverse.Data[3][0])
+	dot0 := NewVec4Arr(row0.ScaleArr(m.Data[0]).Data)
+	det := (dot0.X() + dot0.Y()) + (dot0.Z() + dot0.W())
+
+	inverseDet := 1.0 / det
+
+	m.Data = inverse.Scale(inverseDet).Data
+	return m
+}
+
+// InvertAndTranspose is equivalent to m.Invert().Transpose(), that is invert first, then transpose the inverted matrix.
+//
+// This function is provided as a convenience and as a small optimization, as it inlines the invert and transpose functions which means we only
+// have 1 function call.
+//
+// Additionally, the inverse of the matrix is written to the matrix immediately transposed instead of writing the inverse and then transposing it in a second operation.
+func (m *Mat4) InvertAndTranspose() *Mat4 {
+
+	// Many thanks to the C++ GLM project here :)
+
+	coef00 := m.Data[2][2]*m.Data[3][3] - m.Data[3][2]*m.Data[2][3]
+	coef02 := m.Data[1][2]*m.Data[3][3] - m.Data[3][2]*m.Data[1][3]
+	coef03 := m.Data[1][2]*m.Data[2][3] - m.Data[2][2]*m.Data[1][3]
+
+	coef04 := m.Data[2][1]*m.Data[3][3] - m.Data[3][1]*m.Data[2][3]
+	coef06 := m.Data[1][1]*m.Data[3][3] - m.Data[3][1]*m.Data[1][3]
+	coef07 := m.Data[1][1]*m.Data[2][3] - m.Data[2][1]*m.Data[1][3]
+
+	coef08 := m.Data[2][1]*m.Data[3][2] - m.Data[3][1]*m.Data[2][2]
+	coef10 := m.Data[1][1]*m.Data[3][2] - m.Data[3][1]*m.Data[1][2]
+	coef11 := m.Data[1][1]*m.Data[2][2] - m.Data[2][1]*m.Data[1][2]
+
+	coef12 := m.Data[2][0]*m.Data[3][3] - m.Data[3][0]*m.Data[2][3]
+	coef14 := m.Data[1][0]*m.Data[3][3] - m.Data[3][0]*m.Data[1][3]
+	coef15 := m.Data[1][0]*m.Data[2][3] - m.Data[2][0]*m.Data[1][3]
+
+	coef16 := m.Data[2][0]*m.Data[3][2] - m.Data[3][0]*m.Data[2][2]
+	coef18 := m.Data[1][0]*m.Data[3][2] - m.Data[3][0]*m.Data[1][2]
+	coef19 := m.Data[1][0]*m.Data[2][2] - m.Data[2][0]*m.Data[1][2]
+
+	coef20 := m.Data[2][0]*m.Data[3][1] - m.Data[3][0]*m.Data[2][1]
+	coef22 := m.Data[1][0]*m.Data[3][1] - m.Data[3][0]*m.Data[1][1]
+	coef23 := m.Data[1][0]*m.Data[2][1] - m.Data[2][0]*m.Data[1][1]
+
+	fac0 := NewVec4(coef00, coef00, coef02, coef03)
+	fac1 := NewVec4(coef04, coef04, coef06, coef07)
+	fac2 := NewVec4(coef08, coef08, coef10, coef11)
+	fac3 := NewVec4(coef12, coef12, coef14, coef15)
+	fac4 := NewVec4(coef16, coef16, coef18, coef19)
+	fac5 := NewVec4(coef20, coef20, coef22, coef23)
+
+	vec0 := NewVec4(m.Data[1][0], m.Data[0][0], m.Data[0][0], m.Data[0][0])
+	vec1 := NewVec4(m.Data[1][1], m.Data[0][1], m.Data[0][1], m.Data[0][1])
+	vec2 := NewVec4(m.Data[1][2], m.Data[0][2], m.Data[0][2], m.Data[0][2])
+	vec3 := NewVec4(m.Data[1][3], m.Data[0][3], m.Data[0][3], m.Data[0][3])
+
+	inv0 := NewVec4(
+		vec1.X()*fac0.X()-vec2.X()*fac1.X()+vec3.X()*fac2.X(),
+		vec1.Y()*fac0.Y()-vec2.Y()*fac1.Y()+vec3.Y()*fac2.Y(),
+		vec1.Z()*fac0.Z()-vec2.Z()*fac1.Z()+vec3.Z()*fac2.Z(),
+		vec1.W()*fac0.W()-vec2.W()*fac1.W()+vec3.W()*fac2.W(),
+	)
+
+	inv1 := NewVec4(
+		vec0.X()*fac0.X()-vec2.X()*fac3.X()+vec3.X()*fac4.X(),
+		vec0.Y()*fac0.Y()-vec2.Y()*fac3.Y()+vec3.Y()*fac4.Y(),
+		vec0.Z()*fac0.Z()-vec2.Z()*fac3.Z()+vec3.Z()*fac4.Z(),
+		vec0.W()*fac0.W()-vec2.W()*fac3.W()+vec3.W()*fac4.W(),
+	)
+
+	inv2 := NewVec4(
+		vec0.X()*fac1.X()-vec1.X()*fac3.X()+vec3.X()*fac5.X(),
+		vec0.Y()*fac1.Y()-vec1.Y()*fac3.Y()+vec3.Y()*fac5.Y(),
+		vec0.Z()*fac1.Z()-vec1.Z()*fac3.Z()+vec3.Z()*fac5.Z(),
+		vec0.W()*fac1.W()-vec1.W()*fac3.W()+vec3.W()*fac5.W(),
+	)
+
+	inv3 := NewVec4(
+		vec0.X()*fac2.X()-vec1.X()*fac4.X()+vec2.X()*fac5.X(),
+		vec0.Y()*fac2.Y()-vec1.Y()*fac4.Y()+vec2.Y()*fac5.Y(),
+		vec0.Z()*fac2.Z()-vec1.Z()*fac4.Z()+vec2.Z()*fac5.Z(),
+		vec0.W()*fac2.W()-vec1.W()*fac4.W()+vec2.W()*fac5.W(),
+	)
+
+	signA := NewVec4(+1, -1, +1, -1)
+	signB := NewVec4(-1, +1, -1, +1)
+
+	inverse := NewMat4Arr(
+		inv0.ScaleVec(signA).Data,
+		inv1.ScaleVec(signB).Data,
+		inv2.ScaleVec(signA).Data,
+		inv3.ScaleVec(signB).Data,
+	)
+
+	row0 := NewVec4(inverse.Data[0][0], inverse.Data[1][0], inverse.Data[2][0], inverse.Data[3][0])
+	dot0 := NewVec4Arr(row0.ScaleArr(m.Data[0]).Data)
+	det := (dot0.X() + dot0.Y()) + (dot0.Z() + dot0.W())
+
+	inverseDet := 1.0 / det
+	inverse.Scale(inverseDet)
+
+	// Manually inline transpose
+	m.Data = [4][4]float32{
+		{inverse.Data[0][0], inverse.Data[1][0], inverse.Data[2][0], inverse.Data[3][0]},
+		{inverse.Data[0][1], inverse.Data[1][1], inverse.Data[2][1], inverse.Data[3][1]},
+		{inverse.Data[0][2], inverse.Data[1][2], inverse.Data[2][2], inverse.Data[3][2]},
+		{inverse.Data[0][3], inverse.Data[1][3], inverse.Data[2][3], inverse.Data[3][3]},
+	}
+
+	return m
+}
+
+// ToMat2 returns a Mat2 that contains the top-left 2x2 section of the Mat4.
+// That is, columns 2 and 3, and rows 2 and 3, are dropped.
+func (m *Mat4) ToMat2() Mat2 {
+	return Mat2{
+		Data: [2][2]float32{
+			{m.Data[0][0], m.Data[0][1]},
+			{m.Data[1][0], m.Data[1][1]},
+		},
 	}
 }
 
-//Scale m *= x (element wise multiplication)
-func (m *Mat4) Scale(x float32) {
-
-	m.Data[0] *= x
-	m.Data[1] *= x
-	m.Data[2] *= x
-	m.Data[3] *= x
-
-	m.Data[4] *= x
-	m.Data[5] *= x
-	m.Data[6] *= x
-	m.Data[7] *= x
-
-	m.Data[8] *= x
-	m.Data[9] *= x
-	m.Data[10] *= x
-	m.Data[11] *= x
-
-	m.Data[12] *= x
-	m.Data[13] *= x
-	m.Data[14] *= x
-	m.Data[15] *= x
+// ToMat3 returns a Mat3 that contains the top-left 3x3 section of the Mat4.
+// That is, column 3 and row 3 are dropped.
+func (m *Mat4) ToMat3() Mat3 {
+	return Mat3{
+		Data: [3][3]float32{
+			{m.Data[0][0], m.Data[0][1], m.Data[0][2]},
+			{m.Data[1][0], m.Data[1][1], m.Data[1][2]},
+			{m.Data[2][0], m.Data[2][1], m.Data[2][2]},
+		},
+	}
 }
 
-//AddMat4 m3 = m1 + m2
+// AddMat4 m3 = m1 + m2
 func AddMat4(m1, m2 *Mat4) *Mat4 {
 	return &Mat4{
-		Data: [16]float32{
-			m1.Data[0] + m2.Data[0],
-			m1.Data[1] + m2.Data[1],
-			m1.Data[2] + m2.Data[2],
-			m1.Data[3] + m2.Data[3],
-
-			m1.Data[4] + m2.Data[4],
-			m1.Data[5] + m2.Data[5],
-			m1.Data[6] + m2.Data[6],
-			m1.Data[7] + m2.Data[7],
-
-			m1.Data[8] + m2.Data[8],
-			m1.Data[9] + m2.Data[9],
-			m1.Data[10] + m2.Data[10],
-			m1.Data[11] + m2.Data[11],
-
-			m1.Data[12] + m2.Data[12],
-			m1.Data[13] + m2.Data[13],
-			m1.Data[14] + m2.Data[14],
-			m1.Data[15] + m2.Data[15],
+		Data: [4][4]float32{
+			{
+				m1.Data[0][0] + m2.Data[0][0],
+				m1.Data[0][1] + m2.Data[0][1],
+				m1.Data[0][2] + m2.Data[0][2],
+				m1.Data[0][3] + m2.Data[0][3],
+			},
+			{
+				m1.Data[1][0] + m2.Data[1][0],
+				m1.Data[1][1] + m2.Data[1][1],
+				m1.Data[1][2] + m2.Data[1][2],
+				m1.Data[1][3] + m2.Data[1][3],
+			},
+			{
+				m1.Data[2][0] + m2.Data[2][0],
+				m1.Data[2][1] + m2.Data[2][1],
+				m1.Data[2][2] + m2.Data[2][2],
+				m1.Data[2][3] + m2.Data[2][3],
+			},
+			{
+				m1.Data[3][0] + m2.Data[3][0],
+				m1.Data[3][1] + m2.Data[3][1],
+				m1.Data[3][2] + m2.Data[3][2],
+				m1.Data[3][3] + m2.Data[3][3],
+			},
 		},
 	}
 }
 
-//SubMat4 m3 = m1 - m2
+// SubMat4 m3 = m1 - m2
 func SubMat4(m1, m2 *Mat4) *Mat4 {
 	return &Mat4{
-		Data: [16]float32{
-			m1.Data[0] - m2.Data[0],
-			m1.Data[1] - m2.Data[1],
-			m1.Data[2] - m2.Data[2],
-			m1.Data[3] - m2.Data[3],
-
-			m1.Data[4] - m2.Data[4],
-			m1.Data[5] - m2.Data[5],
-			m1.Data[6] - m2.Data[6],
-			m1.Data[7] - m2.Data[7],
-
-			m1.Data[8] - m2.Data[8],
-			m1.Data[9] - m2.Data[9],
-			m1.Data[10] - m2.Data[10],
-			m1.Data[11] - m2.Data[11],
-
-			m1.Data[12] - m2.Data[12],
-			m1.Data[13] - m2.Data[13],
-			m1.Data[14] - m2.Data[14],
-			m1.Data[15] - m2.Data[15],
+		Data: [4][4]float32{
+			{
+				m1.Data[0][0] - m2.Data[0][0],
+				m1.Data[0][1] - m2.Data[0][1],
+				m1.Data[0][2] - m2.Data[0][2],
+				m1.Data[0][3] - m2.Data[0][3],
+			},
+			{
+				m1.Data[1][0] - m2.Data[1][0],
+				m1.Data[1][1] - m2.Data[1][1],
+				m1.Data[1][2] - m2.Data[1][2],
+				m1.Data[1][3] - m2.Data[1][3],
+			},
+			{
+				m1.Data[2][0] - m2.Data[2][0],
+				m1.Data[2][1] - m2.Data[2][1],
+				m1.Data[2][2] - m2.Data[2][2],
+				m1.Data[2][3] - m2.Data[2][3],
+			},
+			{
+				m1.Data[3][0] - m2.Data[3][0],
+				m1.Data[3][1] - m2.Data[3][1],
+				m1.Data[3][2] - m2.Data[3][2],
+				m1.Data[3][3] - m2.Data[3][3],
+			},
 		},
 	}
 }
 
-//MulMat4 m3 = m1 * m2
+// MulMat4 m3 = m1 * m2
 func MulMat4(m1, m2 *Mat4) *Mat4 {
+
+	m00 := m1.Data[0][0]
+	m01 := m1.Data[0][1]
+	m02 := m1.Data[0][2]
+	m03 := m1.Data[0][3]
+
+	m10 := m1.Data[1][0]
+	m11 := m1.Data[1][1]
+	m12 := m1.Data[1][2]
+	m13 := m1.Data[1][3]
+
+	m20 := m1.Data[2][0]
+	m21 := m1.Data[2][1]
+	m22 := m1.Data[2][2]
+	m23 := m1.Data[2][3]
+
+	m30 := m1.Data[3][0]
+	m31 := m1.Data[3][1]
+	m32 := m1.Data[3][2]
+	m33 := m1.Data[3][3]
+
 	return &Mat4{
-		Data: [16]float32{
-			m1.Data[0]*m2.Data[0] + m1.Data[1]*m2.Data[4] + m1.Data[2]*m2.Data[8] + m1.Data[3]*m2.Data[12],
-			m1.Data[0]*m2.Data[1] + m1.Data[1]*m2.Data[5] + m1.Data[2]*m2.Data[9] + m1.Data[3]*m2.Data[13],
-			m1.Data[0]*m2.Data[2] + m1.Data[1]*m2.Data[6] + m1.Data[2]*m2.Data[10] + m1.Data[3]*m2.Data[14],
-			m1.Data[0]*m2.Data[3] + m1.Data[1]*m2.Data[7] + m1.Data[2]*m2.Data[11] + m1.Data[3]*m2.Data[15],
-
-			m1.Data[4]*m2.Data[0] + m1.Data[5]*m2.Data[4] + m1.Data[6]*m2.Data[8] + m1.Data[7]*m2.Data[12],
-			m1.Data[4]*m2.Data[1] + m1.Data[5]*m2.Data[5] + m1.Data[6]*m2.Data[9] + m1.Data[7]*m2.Data[13],
-			m1.Data[4]*m2.Data[2] + m1.Data[5]*m2.Data[6] + m1.Data[6]*m2.Data[10] + m1.Data[7]*m2.Data[14],
-			m1.Data[4]*m2.Data[3] + m1.Data[5]*m2.Data[7] + m1.Data[6]*m2.Data[11] + m1.Data[7]*m2.Data[15],
-
-			m1.Data[8]*m2.Data[0] + m1.Data[9]*m2.Data[4] + m1.Data[10]*m2.Data[8] + m1.Data[11]*m2.Data[12],
-			m1.Data[8]*m2.Data[1] + m1.Data[9]*m2.Data[5] + m1.Data[10]*m2.Data[9] + m1.Data[11]*m2.Data[13],
-			m1.Data[8]*m2.Data[2] + m1.Data[9]*m2.Data[6] + m1.Data[10]*m2.Data[10] + m1.Data[11]*m2.Data[14],
-			m1.Data[8]*m2.Data[3] + m1.Data[9]*m2.Data[7] + m1.Data[10]*m2.Data[11] + m1.Data[11]*m2.Data[15],
-
-			m1.Data[12]*m2.Data[0] + m1.Data[13]*m2.Data[4] + m1.Data[14]*m2.Data[8] + m1.Data[15]*m2.Data[12],
-			m1.Data[12]*m2.Data[1] + m1.Data[13]*m2.Data[5] + m1.Data[14]*m2.Data[9] + m1.Data[15]*m2.Data[13],
-			m1.Data[12]*m2.Data[2] + m1.Data[13]*m2.Data[6] + m1.Data[14]*m2.Data[10] + m1.Data[15]*m2.Data[14],
-			m1.Data[12]*m2.Data[3] + m1.Data[13]*m2.Data[7] + m1.Data[14]*m2.Data[11] + m1.Data[15]*m2.Data[15],
+		Data: [4][4]float32{
+			{
+				m00*m2.Data[0][0] + m10*m2.Data[0][1] + m20*m2.Data[0][2] + m30*m2.Data[0][3],
+				m01*m2.Data[0][0] + m11*m2.Data[0][1] + m21*m2.Data[0][2] + m31*m2.Data[0][3],
+				m02*m2.Data[0][0] + m12*m2.Data[0][1] + m22*m2.Data[0][2] + m32*m2.Data[0][3],
+				m03*m2.Data[0][0] + m13*m2.Data[0][1] + m23*m2.Data[0][2] + m33*m2.Data[0][3],
+			},
+			{
+				m00*m2.Data[1][0] + m10*m2.Data[1][1] + m20*m2.Data[1][2] + m30*m2.Data[1][3],
+				m01*m2.Data[1][0] + m11*m2.Data[1][1] + m21*m2.Data[1][2] + m31*m2.Data[1][3],
+				m02*m2.Data[1][0] + m12*m2.Data[1][1] + m22*m2.Data[1][2] + m32*m2.Data[1][3],
+				m03*m2.Data[1][0] + m13*m2.Data[1][1] + m23*m2.Data[1][2] + m33*m2.Data[1][3],
+			},
+			{
+				m00*m2.Data[2][0] + m10*m2.Data[2][1] + m20*m2.Data[2][2] + m30*m2.Data[2][3],
+				m01*m2.Data[2][0] + m11*m2.Data[2][1] + m21*m2.Data[2][2] + m31*m2.Data[2][3],
+				m02*m2.Data[2][0] + m12*m2.Data[2][1] + m22*m2.Data[2][2] + m32*m2.Data[2][3],
+				m03*m2.Data[2][0] + m13*m2.Data[2][1] + m23*m2.Data[2][2] + m33*m2.Data[2][3],
+			},
+			{
+				m00*m2.Data[3][0] + m10*m2.Data[3][1] + m20*m2.Data[3][2] + m30*m2.Data[3][3],
+				m01*m2.Data[3][0] + m11*m2.Data[3][1] + m21*m2.Data[3][2] + m31*m2.Data[3][3],
+				m02*m2.Data[3][0] + m12*m2.Data[3][1] + m22*m2.Data[3][2] + m32*m2.Data[3][3],
+				m03*m2.Data[3][0] + m13*m2.Data[3][1] + m23*m2.Data[3][2] + m33*m2.Data[3][3],
+			},
 		},
 	}
 }
 
-//NewMat4Id returns the 4x4 identity matrix
+// MulMat4Vec4 v2 = m1 * v1
+func MulMat4Vec4(m1 *Mat4, v1 *Vec4) *Vec4 {
+	return &Vec4{
+		Data: [4]float32{
+			m1.Data[0][0]*v1.Data[0] + m1.Data[1][0]*v1.Data[1] + m1.Data[2][0]*v1.Data[2] + m1.Data[3][0]*v1.Data[3],
+			m1.Data[0][1]*v1.Data[0] + m1.Data[1][1]*v1.Data[1] + m1.Data[2][1]*v1.Data[2] + m1.Data[3][1]*v1.Data[3],
+			m1.Data[0][2]*v1.Data[0] + m1.Data[1][2]*v1.Data[1] + m1.Data[2][2]*v1.Data[2] + m1.Data[3][2]*v1.Data[3],
+			m1.Data[0][3]*v1.Data[0] + m1.Data[1][3]*v1.Data[1] + m1.Data[2][3]*v1.Data[2] + m1.Data[3][3]*v1.Data[3],
+		},
+	}
+}
+
+// NewMat4Id returns the 4x4 identity matrix
 func NewMat4Id() *Mat4 {
 	return &Mat4{
-		Data: [16]float32{
-			1, 0, 0, 0,
-			0, 1, 0, 0,
-			0, 0, 1, 0,
-			0, 0, 0, 1,
+		Data: [4][4]float32{
+			{1, 0, 0, 0},
+			{0, 1, 0, 0},
+			{0, 0, 1, 0},
+			{0, 0, 0, 1},
+		},
+	}
+}
+
+func NewMat4Diag(diagVal float32) *Mat4 {
+	return &Mat4{
+		Data: [4][4]float32{
+			{diagVal, 0, 0, 0},
+			{0, diagVal, 0, 0},
+			{0, 0, diagVal, 0},
+			{0, 0, 0, diagVal},
+		},
+	}
+}
+
+func NewMat4DiagArr(diag [4]float32) *Mat4 {
+	return &Mat4{
+		Data: [4][4]float32{
+			{diag[0], 0, 0, 0},
+			{0, diag[1], 0, 0},
+			{0, 0, diag[2], 0},
+			{0, 0, 0, diag[3]},
+		},
+	}
+}
+
+func NewMat4Vec4(col0, col1, col2, col3 *Vec4) *Mat4 {
+	return &Mat4{
+		Data: [4][4]float32{
+			col0.Data,
+			col1.Data,
+			col2.Data,
+			col3.Data,
+		},
+	}
+}
+
+func NewMat4Arr(col0, col1, col2, col3 [4]float32) *Mat4 {
+	return &Mat4{
+		Data: [4][4]float32{
+			col0,
+			col1,
+			col2,
+			col3,
 		},
 	}
 }

gglm/mat4_test.go

@@ -0,0 +1,327 @@
+package gglm_test
+
+import (
+	"testing"
+
+	"github.com/bloeys/gglm/gglm"
+)
+
+var (
+	mulMat4Vec4Res *gglm.Vec4
+)
+
+func TestMat4GetSet(t *testing.T) {
+
+	m1 := gglm.Mat4{}
+
+	m1.Set(0, 1, -10)
+	m1.Set(1, 0, 55)
+	m1.Set(2, 2, 99)
+	m1.Set(3, 3, 513)
+
+	if m1.Get(0, 1) != -10 {
+		t.Errorf("Got: %v; Expected: %v", m1.Get(0, 1), -10)
+	}
+
+	if m1.Get(1, 0) != 55 {
+		t.Errorf("Got: %v; Expected: %v", m1.Get(1, 0), 55)
+	}
+
+	if m1.Get(2, 2) != 99 {
+		t.Errorf("Got: %v; Expected: %v", m1.Get(2, 2), 99)
+	}
+
+	if m1.Get(3, 3) != 513 {
+		t.Errorf("Got: %v; Expected: %v", m1.Get(3, 3), 513)
+	}
+}
+
+func TestMat4Id(t *testing.T) {
+
+	correctAns := gglm.Mat4{
+		Data: [4][4]float32{
+			{1, 0, 0, 0},
+			{0, 1, 0, 0},
+			{0, 0, 1, 0},
+			{0, 0, 0, 1},
+		}}
+
+	m1 := gglm.NewMat4Id()
+	if !m1.Eq(&correctAns) {
+		t.Errorf("Got: %v; Expected: %v", m1.String(), correctAns.String())
+	}
+}
+
+func TestSubMat4(t *testing.T) {
+
+	correctAns := gglm.Mat4{
+		Data: [4][4]float32{
+			{-16, -16, -16, -16},
+			{-16, -16, -16, -16},
+			{-16, -16, -16, -16},
+			{-16, -16, -16, -16},
+		}}
+
+	m1 := &gglm.Mat4{
+		Data: [4][4]float32{
+			{1, 5, 9, 13},
+			{2, 6, 10, 14},
+			{3, 7, 11, 15},
+			{4, 8, 12, 16},
+		}}
+	m2 := &gglm.Mat4{
+		Data: [4][4]float32{
+			{17, 21, 25, 29},
+			{18, 22, 26, 30},
+			{19, 23, 27, 31},
+			{20, 24, 28, 32},
+		}}
+
+	result := gglm.SubMat4(m1, m2)
+	m1.Sub(m2)
+
+	if !result.Eq(&correctAns) {
+		t.Errorf("Got: %v; Expected: %v", result.String(), correctAns.String())
+	}
+
+	if !m1.Eq(&correctAns) {
+		t.Errorf("Got: %v; Expected: %v", m1.String(), correctAns.String())
+	}
+}
+
+func TestAddMat4(t *testing.T) {
+
+	correctAns := gglm.Mat4{
+		Data: [4][4]float32{
+			{18, 26, 34, 42},
+			{20, 28, 36, 44},
+			{22, 30, 38, 46},
+			{24, 32, 40, 48},
+		}}
+
+	m1 := &gglm.Mat4{
+		Data: [4][4]float32{
+			{1, 5, 9, 13},
+			{2, 6, 10, 14},
+			{3, 7, 11, 15},
+			{4, 8, 12, 16},
+		}}
+	m2 := &gglm.Mat4{
+		Data: [4][4]float32{
+			{17, 21, 25, 29},
+			{18, 22, 26, 30},
+			{19, 23, 27, 31},
+			{20, 24, 28, 32},
+		}}
+
+	result := gglm.AddMat4(m1, m2)
+	m1.Add(m2)
+
+	if !result.Eq(&correctAns) {
+		t.Errorf("Got: %v; Expected: %v", result.String(), correctAns.String())
+	}
+
+	if !m1.Eq(&correctAns) {
+		t.Errorf("Got: %v; Expected: %v", m1.String(), correctAns.String())
+	}
+}
+
+func TestMulMat4(t *testing.T) {
+
+	correctAns := gglm.Mat4{
+		Data: [4][4]float32{
+			{250, 618, 986, 1354},
+			{260, 644, 1028, 1412},
+			{270, 670, 1070, 1470},
+			{280, 696, 1112, 1528},
+		}}
+
+	m1 := &gglm.Mat4{
+		Data: [4][4]float32{
+			{1, 5, 9, 13},
+			{2, 6, 10, 14},
+			{3, 7, 11, 15},
+			{4, 8, 12, 16},
+		}}
+	m2 := &gglm.Mat4{
+		Data: [4][4]float32{
+			{17, 21, 25, 29},
+			{18, 22, 26, 30},
+			{19, 23, 27, 31},
+			{20, 24, 28, 32},
+		}}
+
+	result := gglm.MulMat4(m1, m2)
+	m1.Mul(m2)
+
+	if !result.Eq(&correctAns) {
+		t.Errorf("Got: %v; Expected: %v", result.String(), correctAns.String())
+	}
+
+	if !m1.Eq(&correctAns) {
+		t.Errorf("Got: %v; Expected: %v", m1.String(), correctAns.String())
+	}
+}
+
+func TestMulMat4Vec4(t *testing.T) {
+
+	m := &gglm.Mat4{
+		Data: [4][4]float32{
+			{1, 5, 9, 13},
+			{2, 6, 10, 14},
+			{3, 7, 11, 15},
+			{4, 8, 12, 16},
+		}}
+	v := &gglm.Vec4{Data: [4]float32{1, 2, 3, 4}}
+
+	result := gglm.MulMat4Vec4(m, v)
+	correctAns := gglm.Vec4{Data: [4]float32{30, 70, 110, 150}}
+
+	if !result.Eq(&correctAns) {
+		t.Errorf("Got: %v; Expected: %v", result.String(), correctAns.String())
+	}
+}
+
+func TestTransposeMat4(t *testing.T) {
+
+	m := gglm.NewMat4Id()
+	ans := gglm.NewMat4Id()
+
+	if !m.Transpose().Transpose().Eq(ans) {
+		t.Errorf("Got: %v; Expected: %v", m.String(), ans.String())
+	}
+
+	if !m.Transpose().Eq(ans) {
+		t.Errorf("Got: %v; Expected: %v", m.String(), ans.String())
+	}
+
+	m = &gglm.Mat4{
+		Data: [4][4]float32{
+			{00, 01, 02, 03},
+			{10, 11, 12, 13},
+			{20, 21, 22, 23},
+			{30, 31, 32, 33},
+		},
+	}
+
+	ans = &gglm.Mat4{
+		Data: [4][4]float32{
+			{00, 10, 20, 30},
+			{01, 11, 21, 31},
+			{02, 12, 22, 32},
+			{03, 13, 23, 33},
+		},
+	}
+
+	if !m.Transpose().Eq(ans) {
+		t.Errorf("Got: %v; Expected: %v", m.String(), ans.String())
+	}
+}
+
+func TestDeterminantMat4(t *testing.T) {
+
+	m := gglm.NewMat4Id()
+	ans := float32(1)
+
+	if m.Determinant() != ans {
+		t.Errorf("Got: %f; Expected: %f", m.Determinant(), ans)
+	}
+
+	m = &gglm.Mat4{
+		Data: [4][4]float32{
+			{1, 0, 2, 1},
+			{2, 1, 3, 0},
+			{3, 0, 4, 1},
+			{4, 1, 5, 1},
+		},
+	}
+	ans = -2
+
+	if m.Determinant() != ans {
+		t.Errorf("Got: %f; Expected: %f", m.Determinant(), ans)
+	}
+}
+
+func TestInvertMat4(t *testing.T) {
+
+	m := gglm.NewMat4Id()
+	ans := gglm.NewMat4Id()
+
+	if !m.Invert().Eq(ans) {
+		t.Errorf("Got: %v; Expected: %v", m.String(), ans.String())
+	}
+
+	m = &gglm.Mat4{
+		Data: [4][4]float32{
+			{1, 0, 2, 1},
+			{2, 1, 3, 0},
+			{3, 0, 4, 1},
+			{4, 1, 5, 1},
+		},
+	}
+
+	ans = &gglm.Mat4{
+		Data: [4][4]float32{
+			{-1, -1, 0, 1},
+			{0.5, 0, -3 / 2.0, 1},
+			{0.5, 1, 0.5, -1},
+			{1, -1, -1, 1},
+		},
+	}
+
+	if !m.Invert().Eq(ans) {
+		t.Errorf("Got: %v\nExpected: %v;\n", m.String(), ans.String())
+	}
+}
+
+func TestInvertAndTransposeMat4(t *testing.T) {
+
+	m := gglm.NewMat4Id()
+	ans := gglm.NewMat4Id().Transpose()
+
+	if !m.InvertAndTranspose().Eq(ans) {
+		t.Errorf("Got: %v; Expected: %v", m.String(), ans.String())
+	}
+
+	m = &gglm.Mat4{
+		Data: [4][4]float32{
+			{1, 0, 2, 1},
+			{2, 1, 3, 0},
+			{3, 0, 4, 1},
+			{4, 1, 5, 1},
+		},
+	}
+
+	ans = &gglm.Mat4{
+		Data: [4][4]float32{
+			{-1, -1, 0, 1},
+			{0.5, 0, -3 / 2.0, 1},
+			{0.5, 1, 0.5, -1},
+			{1, -1, -1, 1},
+		},
+	}
+
+	if !m.InvertAndTranspose().Eq(ans.Transpose()) {
+		t.Errorf("Got: %v\nExpected: %v;\n", m.String(), ans.String())
+	}
+}
+
+func BenchmarkMulMat4(b *testing.B) {
+
+	m1 := gglm.NewMat4Id()
+	m2 := gglm.NewMat4Id()
+
+	for i := 0; i < b.N; i++ {
+		m1.Mul(m2)
+	}
+}
+
+func BenchmarkMulMat4Vec4(b *testing.B) {
+
+	m1 := gglm.NewMat4Id()
+	v1 := gglm.Vec4{}
+
+	for i := 0; i < b.N; i++ {
+		mulMat4Vec4Res = gglm.MulMat4Vec4(m1, &v1)
+	}
+}

gglm/quat.go

@@ -0,0 +1,126 @@
+package gglm
+
+import (
+	"fmt"
+)
+
+var _ Swizzle4 = &Quat{}
+var _ fmt.Stringer = &Quat{}
+
+type Quat struct {
+	Vec4
+}
+
+// Eq checks for exact equality
+func (q *Quat) Eq(q2 *Quat) bool {
+	return q.Data == q2.Data
+}
+
+// Angle returns the angle represented by this quaternion in radians
+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 {
+
+	//Some other common terminology: x=roll, y=pitch, z=yaw
+	sinX, cosX := Sincos32(v.Data[0] * 0.5)
+	sinY, cosY := Sincos32(v.Data[1] * 0.5)
+	sinZ, cosZ := Sincos32(v.Data[2] * 0.5)
+
+	//This produces a z->y->x multiply order, but its written as XYZ.
+	//This is due to XYZ meaning independent rotation matrices, so Z is applied
+	//first, then Y matrix and lastly X.
+	//See this for more info: https://github.com/godotengine/godot/issues/6816#issuecomment-254592170
+	//
+	//Note: On most conversion tools putting the multiply order (e.g. ZYX for us) is required.
+	return &Quat{
+		Vec4: Vec4{
+			Data: [4]float32{
+				sinX*cosY*cosZ - cosX*sinY*sinZ,
+				cosX*sinY*cosZ + sinX*cosY*sinZ,
+				cosX*cosY*sinZ - sinX*sinY*cosZ,
+				cosX*cosY*cosZ + sinX*sinY*sinZ,
+			},
+		},
+	}
+}
+
+// Euler takes rotations in radians and produces a rotation that
+// rotates around the z-axis, y-axis and lastly x-axis.
+func NewQuatEulerXYZ(x, y, z float32) *Quat {
+
+	//Some other common terminology: x=roll, y=pitch, z=yaw
+	sinX, cosX := Sincos32(x * 0.5)
+	sinY, cosY := Sincos32(y * 0.5)
+	sinZ, cosZ := Sincos32(z * 0.5)
+
+	//This produces a z->y->x multiply order, but its written as XYZ.
+	//This is due to XYZ meaning independent rotation matrices, so Z is applied
+	//first, then Y matrix and lastly X.
+	//See this for more info: https://github.com/godotengine/godot/issues/6816#issuecomment-254592170
+	//
+	//Note: On most conversion tools putting the multiply order (e.g. ZYX for us) is required.
+	return &Quat{
+		Vec4: Vec4{
+			Data: [4]float32{
+				sinX*cosY*cosZ - cosX*sinY*sinZ,
+				cosX*sinY*cosZ + sinX*cosY*sinZ,
+				cosX*cosY*sinZ - sinX*sinY*cosZ,
+				cosX*cosY*cosZ + sinX*sinY*sinZ,
+			},
+		},
+	}
+}
+
+// NewQuatAngleAxis produces a quaternion thats rotates rotRad radians around the *normalized* vector rotAxisNorm
+func NewQuatAngleAxis(rotRad float32, rotAxisNorm *Vec3) *Quat {
+
+	s, c := Sincos32(rotRad * 0.5)
+	return &Quat{
+		Vec4: Vec4{
+			Data: [4]float32{
+				rotAxisNorm.Data[0] * s,
+				rotAxisNorm.Data[1] * s,
+				rotAxisNorm.Data[2] * s,
+				c,
+			},
+		},
+	}
+}
+
+func NewQuatId() *Quat {
+	return &Quat{
+		Vec4: Vec4{
+			Data: [4]float32{0, 0, 0, 1},
+		},
+	}
+}

gglm/quat_test.go

@@ -0,0 +1,81 @@
+package gglm_test
+
+import (
+	"testing"
+
+	"github.com/bloeys/gglm/gglm"
+)
+
+func TestNewQuatEuler(t *testing.T) {
+
+	q := gglm.NewQuatEuler(gglm.NewVec3(180, 180, 180).AsRad())
+	ans := &gglm.Quat{Vec4: *gglm.NewVec4(0, 0, 0, 1)}
+
+	if !gglm.EqF32(q.X(), ans.X()) || !gglm.EqF32(q.Y(), ans.Y()) || !gglm.EqF32(q.Z(), ans.Z()) || !gglm.EqF32(q.W(), ans.W()) {
+		t.Errorf("Got: %v; Expected: %v", q.String(), ans.String())
+	}
+
+	q = gglm.NewQuatEulerXYZ(180*gglm.Deg2Rad, 180*gglm.Deg2Rad, 180*gglm.Deg2Rad)
+
+	if !gglm.EqF32(q.X(), ans.X()) || !gglm.EqF32(q.Y(), ans.Y()) || !gglm.EqF32(q.Z(), ans.Z()) || !gglm.EqF32(q.W(), ans.W()) {
+		t.Errorf("Got: %v; Expected: %v", q.String(), ans.String())
+	}
+}
+
+func TestNewQuatAngleAxis(t *testing.T) {
+
+	q := gglm.NewQuatAngleAxis(180*gglm.Deg2Rad, gglm.NewVec3(0, 1, 0))
+	ans := &gglm.Quat{Vec4: *gglm.NewVec4(0, 1, 0, 0)}
+
+	if !gglm.EqF32(q.X(), ans.X()) || !gglm.EqF32(q.Y(), ans.Y()) || !gglm.EqF32(q.Z(), ans.Z()) || !gglm.EqF32(q.W(), ans.W()) {
+		t.Errorf("Got: %v; Expected: %v", q.String(), ans.String())
+	}
+}
+
+func TestQuatAngle(t *testing.T) {
+
+	a := gglm.NewQuatAngleAxis(180*gglm.Deg2Rad, gglm.NewVec3(0, 1, 0)).Angle()
+	var ans float32 = 180.0 * gglm.Deg2Rad
+
+	if !gglm.EqF32(a, ans) {
+		t.Errorf("Got: %v; Expected: %v", a, ans)
+	}
+
+	a = gglm.NewQuatAngleAxis(90*gglm.Deg2Rad, gglm.NewVec3(1, 1, 0).Normalize()).Angle()
+	ans = 90 * gglm.Deg2Rad
+
+	if !gglm.EqF32(a, ans) {
+		t.Errorf("Got: %v; Expected: %v", a, ans)
+	}
+
+	a = gglm.NewQuatAngleAxis(125*gglm.Deg2Rad, gglm.NewVec3(1, 1, 0).Normalize()).Angle()
+	ans = 125 * gglm.Deg2Rad
+
+	if !gglm.EqF32(a, ans) {
+		t.Errorf("Got: %v; Expected: %v", a, ans)
+	}
+}
+
+func TestQuatAxis(t *testing.T) {
+
+	a := gglm.NewQuatAngleAxis(1, gglm.NewVec3(0, 1, 0)).Axis()
+	ans := gglm.NewVec3(0, 1, 0)
+
+	if !gglm.EqF32(a.X(), ans.X()) || !gglm.EqF32(a.Y(), ans.Y()) || !gglm.EqF32(a.Z(), ans.Z()) {
+		t.Errorf("Got: %v; Expected: %v", a.String(), ans.String())
+	}
+
+	a = gglm.NewQuatAngleAxis(1, gglm.NewVec3(1, 1, 0).Normalize()).Axis()
+	ans = gglm.NewVec3(1, 1, 0).Normalize()
+
+	if !gglm.EqF32(a.X(), ans.X()) || !gglm.EqF32(a.Y(), ans.Y()) || !gglm.EqF32(a.Z(), ans.Z()) {
+		t.Errorf("Got: %v; Expected: %v", a.String(), ans.String())
+	}
+
+	a = gglm.NewQuatAngleAxis(1, gglm.NewVec3(67, 46, 32).Normalize()).Axis()
+	ans = gglm.NewVec3(67, 46, 32).Normalize()
+
+	if !gglm.EqF32(a.X(), ans.X()) || !gglm.EqF32(a.Y(), ans.Y()) || !gglm.EqF32(a.Z(), ans.Z()) {
+		t.Errorf("Got: %v; Expected: %v", a.String(), ans.String())
+	}
+}

gglm/scalar.go

@@ -0,0 +1,54 @@
+package gglm
+
+import "math"
+
+// EqF32 true if abs(f1-f2) <= F32Epsilon
+func EqF32(f1, f2 float32) bool {
+	return math.Abs(float64(f1-f2)) <= float64(F32Epsilon)
+}
+
+// EqF32Epsilon true if abs(f1-f2) <= eps
+func EqF32Epsilon(f1, f2, eps float32) bool {
+	return math.Abs(float64(f1-f2)) <= float64(eps)
+}
+
+func Sin32(x float32) float32 {
+	return float32(math.Sin(float64(x)))
+}
+
+func Asin32(x float32) float32 {
+	return float32(math.Asin(float64(x)))
+}
+
+func Cos32(x float32) float32 {
+	return float32(math.Cos(float64(x)))
+}
+
+func Acos32(x float32) float32 {
+	return float32(math.Acos(float64(x)))
+}
+
+func Tan32(x float32) float32 {
+	return float32(math.Tan(float64(x)))
+}
+
+func Atan32(x float32) float32 {
+	return float32(math.Atan(float64(x)))
+}
+
+func Atan232(x, y float32) float32 {
+	return float32(math.Atan2(float64(y), float64(x)))
+}
+
+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)))
+}

gglm/swizzle.go

@@ -2,24 +2,64 @@ package gglm
 
 type Swizzle1 interface {
 	X() float32
+	SetX(float32)
+	AddX(float32)
+
 	R() float32
+	SetR(float32)
+	AddR(float32)
 }
 
 type Swizzle2 interface {
 	Swizzle1
+
 	Y() float32
+	SetY(float32)
+	AddY(float32)
+
+	SetXY(float32, float32)
+	AddXY(float32, float32)
+
 	G() float32
+	SetG(float32)
+	AddG(float32)
+
+	SetRG(float32, float32)
+	AddRG(float32, float32)
 }
 
 type Swizzle3 interface {
 	Swizzle2
-	Z() float32
-	B() float32
 
-	// XYZ
-	// YXZ
-	// XZY
-	// YZX
-	// ZXY
-	// ZYX
+	Z() float32
+	SetZ(float32)
+	AddZ(float32)
+
+	SetXYZ(float32, float32, float32)
+	AddXYZ(float32, float32, float32)
+
+	B() float32
+	SetB(float32)
+	AddB(float32)
+
+	SetRGB(float32, float32, float32)
+	AddRGB(float32, float32, float32)
+}
+
+type Swizzle4 interface {
+	Swizzle3
+
+	W() float32
+	SetW(float32)
+	AddW(float32)
+
+	SetXYZW(float32, float32, float32, float32)
+	AddXYZW(float32, float32, float32, float32)
+
+	A() float32
+	SetA(float32)
+	AddA(float32)
+
+	SetRGBA(float32, float32, float32, float32)
+	AddRGBA(float32, float32, float32, float32)
 }

gglm/transform.go

@@ -0,0 +1,219 @@
+package gglm
+
+import (
+	"fmt"
+	"math"
+)
+
+var _ Mat = &TrMat{}
+var _ fmt.Stringer = &TrMat{}
+
+// TrMat represents a transformation matrix
+type TrMat struct {
+	Mat4
+}
+
+// Translate adds v to the translation components of the transformation matrix
+func (t *TrMat) Translate(v *Vec3) *TrMat {
+	t.Data[3][0] += v.Data[0]
+	t.Data[3][1] += v.Data[1]
+	t.Data[3][2] += v.Data[2]
+	return t
+}
+
+// Scale multiplies the scale components of the transformation matrix by v
+func (t *TrMat) Scale(v *Vec3) *TrMat {
+	t.Data[0][0] *= v.Data[0]
+	t.Data[1][1] *= v.Data[1]
+	t.Data[2][2] *= v.Data[2]
+	return t
+}
+
+// Rotate takes a *normalized* axis and angles in radians to rotate around the given axis
+func (t *TrMat) Rotate(rads float32, axis *Vec3) *TrMat {
+
+	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]
+	return t
+}
+
+func (t *TrMat) Mul(m *TrMat) *TrMat {
+	t.Mat4.Mul(&m.Mat4)
+	return t
+}
+
+func (t *TrMat) Eq(m *TrMat) bool {
+	return t.Data == m.Data
+}
+
+func (t *TrMat) Clone() *TrMat {
+	return &TrMat{
+		Mat4: *t.Mat4.Clone(),
+	}
+}
+
+func NewTranslationMat(v *Vec3) *TrMat {
+	return &TrMat{
+		Mat4: Mat4{
+			Data: [4][4]float32{
+				{1, 0, 0, 0},
+				{0, 1, 0, 0},
+				{0, 0, 1, 0},
+				{v.Data[0], v.Data[1], v.Data[2], 1},
+			},
+		},
+	}
+}
+
+func NewScaleMat(v *Vec3) *TrMat {
+	return &TrMat{
+		Mat4: Mat4{
+			Data: [4][4]float32{
+				{v.Data[0], 0, 0, 0},
+				{0, v.Data[1], 0, 0},
+				{0, 0, v.Data[2], 0},
+				{0, 0, 0, 1},
+			},
+		},
+	}
+}
+
+func NewRotMat(q *Quat) *TrMat {
+
+	//Based on: https://www.weizmann.ac.il/sci-tea/benari/sites/sci-tea.benari/files/uploads/softwareAndLearningMaterials/quaternion-tutorial-2-0-1.pdf
+	//Note: in the reference p0,p1,p2,p3 == w,x,y,z
+
+	xx := q.Data[0] * q.Data[0]
+	yy := q.Data[1] * q.Data[1]
+	zz := q.Data[2] * q.Data[2]
+	ww := q.Data[3] * q.Data[3]
+
+	xy := q.Data[0] * q.Data[1]
+	xz := q.Data[0] * q.Data[2]
+	xw := q.Data[0] * q.Data[3]
+
+	yz := q.Data[1] * q.Data[2]
+	yw := q.Data[1] * q.Data[3]
+
+	zw := q.Data[2] * q.Data[3]
+
+	return &TrMat{
+		Mat4: Mat4{
+			Data: [4][4]float32{
+				{2*(ww+xx) - 1, 2 * (zw + xy), 2 * (xz - yw), 0},
+				{2 * (xy - zw), 2*(ww+yy) - 1, 2 * (xw + yz), 0},
+				{2 * (yw + xz), 2 * (yz - xw), 2*(ww+zz) - 1, 0},
+				{0, 0, 0, 1},
+			},
+		},
+	}
+}
+
+// LookAtRH does a right-handed coordinate system lookAt (RH is the default for OpenGL).
+// Can be used to create the view matrix
+func LookAtRH(pos, targetPos, worldUp *Vec3) *TrMat {
+
+	forward := SubVec3(targetPos, pos).Normalize()
+	right := Cross(forward, worldUp).Normalize()
+	up := Cross(right, forward)
+
+	return &TrMat{
+		Mat4: Mat4{
+			Data: [4][4]float32{
+				{right.Data[0], up.Data[0], -forward.Data[0], 0},
+				{right.Data[1], up.Data[1], -forward.Data[1], 0},
+				{right.Data[2], up.Data[2], -forward.Data[2], 0},
+				{-DotVec3(pos, right), -DotVec3(pos, up), DotVec3(pos, forward), 1},
+			},
+		},
+	}
+}
+
+// LookAtLH does a left-handed coordinate system lookAt.
+// Can be used to create the view matrix
+func LookAtLH(pos, targetPos, worldUp *Vec3) *TrMat {
+
+	forward := SubVec3(targetPos, pos).Normalize()
+	right := Cross(worldUp, forward).Normalize()
+	up := Cross(forward, right)
+
+	return &TrMat{
+		Mat4: Mat4{
+			Data: [4][4]float32{
+				{right.Data[0], up.Data[0], forward.Data[0], 0},
+				{right.Data[1], up.Data[1], forward.Data[1], 0},
+				{right.Data[2], up.Data[2], forward.Data[2], 0},
+				{-DotVec3(pos, right), -DotVec3(pos, up), -DotVec3(pos, forward), 1},
+			},
+		},
+	}
+}
+
+// Perspective creates a perspective projection matrix
+func Perspective(fov, aspectRatio, nearClip, farClip float32) *Mat4 {
+	halfFovTan := float32(math.Tan(float64(fov * 0.5)))
+	return &Mat4{
+		Data: [4][4]float32{
+			{1 / (aspectRatio * halfFovTan), 0, 0, 0},
+			{0, 1 / halfFovTan, 0, 0},
+			{0, 0, -(nearClip + farClip) / (farClip - nearClip), -1},
+			{0, 0, -(2 * farClip * nearClip) / (farClip - nearClip), 0},
+		},
+	}
+}
+
+// Perspective creates an orthographic projection matrix
+func Ortho(left, right, top, bottom, nearClip, farClip float32) *TrMat {
+	return &TrMat{
+		Mat4: Mat4{
+			Data: [4][4]float32{
+				{2 / (right - left), 0, 0, 0},
+				{0, 2 / (top - bottom), 0, 0},
+				{0, 0, -2 / (farClip - nearClip), 0},
+				{-(right + left) / (right - left), -(top + bottom) / (top - bottom), -(farClip + nearClip) / (farClip - nearClip), 1},
+			},
+		},
+	}
+}
+
+func NewTrMatId() *TrMat {
+	return &TrMat{
+		Mat4: *NewMat4Id(),
+	}
+}

gglm/transform_test.go

@@ -0,0 +1,83 @@
+package gglm_test
+
+import (
+	"testing"
+
+	"github.com/bloeys/gglm/gglm"
+)
+
+func TestNewTrMatId(t *testing.T) {
+
+	m := gglm.NewTrMatId()
+	ans := &gglm.TrMat{
+		Mat4: gglm.Mat4{
+			Data: [4][4]float32{
+				{1, 0, 0, 0},
+				{0, 1, 0, 0},
+				{0, 0, 1, 0},
+				{0, 0, 0, 1},
+			},
+		},
+	}
+
+	if !m.Eq(ans) {
+		t.Errorf("Got: %v; Expected: %v", m.String(), ans.String())
+	}
+}
+
+func TestNewTranslationMat(t *testing.T) {
+
+	m := gglm.NewTranslationMat(gglm.NewVec3(1, 2, 3))
+	ans := &gglm.TrMat{
+		Mat4: gglm.Mat4{
+			Data: [4][4]float32{
+				{1, 0, 0, 0},
+				{0, 1, 0, 0},
+				{0, 0, 1, 0},
+				{1, 2, 3, 1},
+			},
+		},
+	}
+
+	if !m.Eq(ans) {
+		t.Errorf("Got: %v; Expected: %v", m.String(), ans.String())
+	}
+}
+
+func TestNewScaleMat(t *testing.T) {
+
+	m := gglm.NewScaleMat(gglm.NewVec3(1, 2, 3))
+	ans := &gglm.TrMat{
+		Mat4: gglm.Mat4{
+			Data: [4][4]float32{
+				{1, 0, 0, 0},
+				{0, 2, 0, 0},
+				{0, 0, 3, 0},
+				{0, 0, 0, 1},
+			},
+		},
+	}
+
+	if !m.Eq(ans) {
+		t.Errorf("Got: %v; Expected: %v", m.String(), ans.String())
+	}
+}
+
+func TestNewRotMat(t *testing.T) {
+
+	m := gglm.NewRotMat(gglm.NewQuatId())
+	ans := &gglm.TrMat{
+		Mat4: gglm.Mat4{
+			Data: [4][4]float32{
+				{1, 0, 0, 0},
+				{0, 1, 0, 0},
+				{0, 0, 1, 0},
+				{0, 0, 0, 1},
+			},
+		},
+	}
+
+	if !m.Eq(ans) {
+		t.Errorf("Got: %v; Expected: %v", m.String(), ans.String())
+	}
+}

gglm/vec2.go

@@ -30,39 +30,127 @@ func (v *Vec2) G() float32 {
 	return v.Data[1]
 }
 
+func (v *Vec2) SetX(x float32) {
+	v.Data[0] = x
+}
+
+func (v *Vec2) SetR(r float32) {
+	v.Data[0] = r
+}
+
+func (v *Vec2) SetY(y float32) {
+	v.Data[1] = y
+}
+
+func (v *Vec2) SetG(g float32) {
+	v.Data[1] = g
+}
+
+func (v *Vec2) AddX(x float32) {
+	v.Data[0] += x
+}
+
+func (v *Vec2) AddY(y float32) {
+	v.Data[1] += y
+}
+
+func (v *Vec2) AddR(r float32) {
+	v.Data[0] += r
+}
+
+func (v *Vec2) AddG(g float32) {
+	v.Data[1] += g
+}
+
+func (v *Vec2) SetXY(x, y float32) {
+	v.Data[0] = x
+	v.Data[1] = y
+}
+
+func (v *Vec2) AddXY(x, y float32) {
+	v.Data[0] += x
+	v.Data[1] += y
+}
+
+func (v *Vec2) SetRG(r, g float32) {
+	v.Data[0] = r
+	v.Data[1] = g
+}
+
+func (v *Vec2) AddRG(r, g float32) {
+	v.Data[0] += r
+	v.Data[1] += g
+}
+
 func (v *Vec2) String() string {
 	return fmt.Sprintf("(%f, %f)", v.X(), v.Y())
 }
 
-//Scale v *= x (element wise multiplication)
-func (v *Vec2) Scale(x float32) {
+// Scale v *= x (element wise multiplication)
+func (v *Vec2) Scale(x float32) *Vec2 {
 	v.Data[0] *= x
 	v.Data[1] *= x
+	return v
 }
 
-//Add v += v2
-func (v *Vec2) Add(v2 *Vec2) {
+// ScaleVec v *= v2 (element wise multiplication)
+func (v *Vec2) ScaleVec(v2 *Vec2) *Vec2 {
+	v.Data[0] *= v2.X()
+	v.Data[1] *= v2.Y()
+	return v
+}
+
+// ScaleArr v *= arr (element wise multiplication)
+func (v *Vec2) ScaleArr(arr [2]float32) *Vec2 {
+	v.Data[0] *= arr[0]
+	v.Data[1] *= arr[1]
+	return v
+}
+
+// Add v += v2
+func (v *Vec2) Add(v2 *Vec2) *Vec2 {
 	v.Data[0] += v2.X()
 	v.Data[1] += v2.Y()
+	return v
 }
 
-//SubVec2 v -= v2
-func (v *Vec2) Sub(v2 *Vec2) {
+// SubVec2 v -= v2
+func (v *Vec2) Sub(v2 *Vec2) *Vec2 {
 	v.Data[0] -= v2.X()
 	v.Data[1] -= v2.Y()
+	return v
 }
 
-//Mag returns the magnitude of the vector
+// Mag returns the magnitude of the vector
 func (v *Vec2) Mag() float32 {
 	return float32(math.Sqrt(float64(v.X()*v.X() + v.Y()*v.Y())))
 }
 
-//Mag returns the squared magnitude of the vector
+// Mag returns the squared magnitude of the vector
 func (v *Vec2) SqrMag() float32 {
 	return v.X()*v.X() + v.Y()*v.Y()
 }
 
-//AddVec2 v3 = v1 + v2
+func (v *Vec2) Eq(v2 *Vec2) bool {
+	return v.Data == v2.Data
+}
+
+func (v *Vec2) Set(x, y float32) {
+	v.Data[0] = x
+	v.Data[1] = y
+}
+
+func (v *Vec2) Normalize() {
+	mag := v.Mag()
+	v.Data[0] /= mag
+	v.Data[1] /= mag
+}
+
+func (v *Vec2) Clone() *Vec2 {
+	return &Vec2{Data: v.Data}
+}
+
+// AddVec2 v3 = v1 + v2
 func AddVec2(v1, v2 *Vec2) *Vec2 {
 	return &Vec2{
 		Data: [2]float32{
@@ -72,7 +160,7 @@ func AddVec2(v1, v2 *Vec2) *Vec2 {
 	}
 }
 
-//SubVec2 v3 = v1 - v2
+// SubVec2 v3 = v1 - v2
 func SubVec2(v1, v2 *Vec2) *Vec2 {
 	return &Vec2{
 		Data: [2]float32{
@@ -81,3 +169,18 @@ func SubVec2(v1, v2 *Vec2) *Vec2 {
 		},
 	}
 }
+
+func NewVec2(x, y float32) *Vec2 {
+	return &Vec2{
+		[2]float32{
+			x,
+			y,
+		},
+	}
+}
+
+func NewVec2Arr(arr [2]float32) *Vec2 {
+	return &Vec2{
+		Data: arr,
+	}
+}

gglm/vec3.go

@@ -36,42 +36,207 @@ func (v *Vec3) B() float32 {
 	return v.Data[2]
 }
 
+func (v *Vec3) SetX(f float32) {
+	v.Data[0] = f
+}
+
+func (v *Vec3) SetR(f float32) {
+	v.Data[0] = f
+}
+
+func (v *Vec3) SetY(f float32) {
+	v.Data[1] = f
+}
+
+func (v *Vec3) SetG(f float32) {
+	v.Data[1] = f
+}
+
+func (v *Vec3) SetZ(f float32) {
+	v.Data[2] = f
+}
+
+func (v *Vec3) SetB(f float32) {
+	v.Data[2] = f
+}
+
+func (v *Vec3) AddX(x float32) {
+	v.Data[0] += x
+}
+
+func (v *Vec3) AddY(y float32) {
+	v.Data[1] += y
+}
+
+func (v *Vec3) AddZ(z float32) {
+	v.Data[2] += z
+}
+
+func (v *Vec3) AddR(r float32) {
+	v.Data[0] += r
+}
+
+func (v *Vec3) AddG(g float32) {
+	v.Data[1] += g
+}
+
+func (v *Vec3) AddB(b float32) {
+	v.Data[2] += b
+}
+
+func (v *Vec3) SetXY(x, y float32) {
+	v.Data[0] = x
+	v.Data[1] = y
+}
+
+func (v *Vec3) AddXY(x, y float32) {
+	v.Data[0] += x
+	v.Data[1] += y
+}
+
+func (v *Vec3) SetRG(r, g float32) {
+	v.Data[0] = r
+	v.Data[1] = g
+}
+
+func (v *Vec3) AddRG(r, g float32) {
+	v.Data[0] += r
+	v.Data[1] += g
+}
+
+func (v *Vec3) SetXYZ(x, y, z float32) {
+	v.Data[0] = x
+	v.Data[1] = y
+	v.Data[2] = z
+}
+
+func (v *Vec3) AddXYZ(x, y, z float32) {
+	v.Data[0] += x
+	v.Data[1] += y
+	v.Data[2] += z
+}
+
+func (v *Vec3) SetRGB(r, g, b float32) {
+	v.Data[0] = r
+	v.Data[1] = g
+	v.Data[2] = b
+}
+
+func (v *Vec3) AddRGB(r, g, b float32) {
+	v.Data[0] += r
+	v.Data[1] += g
+	v.Data[2] += b
+}
+
 func (v *Vec3) String() string {
 	return fmt.Sprintf("(%f, %f, %f)", v.X(), v.Y(), v.Z())
 }
 
-//Scale v *= x (element wise multiplication)
-func (v *Vec3) Scale(x float32) {
+// Scale v *= x (element wise multiplication)
+func (v *Vec3) Scale(x float32) *Vec3 {
 	v.Data[0] *= x
 	v.Data[1] *= x
 	v.Data[2] *= x
+	return v
 }
 
-func (v *Vec3) Add(v2 *Vec3) {
+// ScaleVec v *= v2 (element wise multiplication)
+func (v *Vec3) ScaleVec(v2 *Vec3) *Vec3 {
+	v.Data[0] *= v2.X()
+	v.Data[1] *= v2.Y()
+	v.Data[2] *= v2.Z()
+	return v
+}
+
+// ScaleArr v *= arr (element wise multiplication)
+func (v *Vec3) ScaleArr(arr [3]float32) *Vec3 {
+	v.Data[0] *= arr[0]
+	v.Data[1] *= arr[1]
+	v.Data[2] *= arr[2]
+	return v
+}
+
+func (v *Vec3) Add(v2 *Vec3) *Vec3 {
 
 	v.Data[0] += v2.X()
 	v.Data[1] += v2.Y()
 	v.Data[2] += v2.Z()
+	return v
 }
 
-//SubVec3 v -= v2
-func (v *Vec3) Sub(v2 *Vec3) {
+// SubVec3 v -= v2
+func (v *Vec3) Sub(v2 *Vec3) *Vec3 {
 	v.Data[0] -= v2.X()
 	v.Data[1] -= v2.Y()
 	v.Data[2] -= v2.Z()
+	return v
 }
 
-//Mag returns the magnitude of the vector
+// Mag returns the magnitude of the vector
 func (v *Vec3) Mag() float32 {
 	return float32(math.Sqrt(float64(v.X()*v.X() + v.Y()*v.Y() + v.Z()*v.Z())))
 }
 
-//Mag returns the squared magnitude of the vector
+// Mag returns the squared magnitude of the vector
 func (v *Vec3) SqrMag() float32 {
 	return v.X()*v.X() + v.Y()*v.Y() + v.Z()*v.Z()
 }
 
-//AddVec3 v3 = v1 + v2
+func (v *Vec3) Eq(v2 *Vec3) bool {
+	return v.Data == v2.Data
+}
+
+func (v *Vec3) Set(x, y, z float32) {
+	v.Data[0] = x
+	v.Data[1] = y
+	v.Data[2] = z
+}
+
+// Normalize normalizes this vector and returns it (doesn't copy)
+func (v *Vec3) Normalize() *Vec3 {
+	mag := float32(math.Sqrt(float64(v.X()*v.X() + v.Y()*v.Y() + v.Z()*v.Z())))
+	v.Data[0] /= mag
+	v.Data[1] /= mag
+	v.Data[2] /= mag
+
+	return v
+}
+
+// RotByQuat rotates this vector by the given quaternion
+func (v *Vec3) RotByQuat(q *Quat) *Vec3 {
+
+	// Reference: https://gamedev.stackexchange.com/questions/28395/rotating-vector3-by-a-quaternion
+	// u := NewVec3(q.X(), q.Y(), q.Z())
+	// t1 := 2.0f * dot(u, v) * u
+	// t2 := (s*s - dot(u, u)) * v
+	// t3 := 2.0f * s * cross(u, v);
+	// vprime = t1 + t2 + t3
+
+	u := NewVec3(q.X(), q.Y(), q.Z())
+	t1 := u.Clone().Scale(2 * DotVec3(u, v))
+	t2 := v.Clone().Scale(q.W()*q.W() - DotVec3(u, u))
+	t3 := Cross(u, v).Scale(2 * q.W())
+
+	v.Data = t1.Add(t2).Add(t3).Data
+	return v
+}
+
+func (v *Vec3) Clone() *Vec3 {
+	return &Vec3{Data: v.Data}
+}
+
+// AsRad returns a new vector with all values converted to Radians (i.e. multiplied by gglm.Deg2Rad)
+func (v *Vec3) AsRad() *Vec3 {
+	return &Vec3{
+		Data: [3]float32{
+			v.Data[0] * Deg2Rad,
+			v.Data[1] * Deg2Rad,
+			v.Data[2] * Deg2Rad,
+		},
+	}
+}
+
+// AddVec3 v3 = v1 + v2
 func AddVec3(v1, v2 *Vec3) *Vec3 {
 	return &Vec3{
 		Data: [3]float32{
@@ -82,7 +247,7 @@ func AddVec3(v1, v2 *Vec3) *Vec3 {
 	}
 }
 
-//SubVec3 v3 = v1 - v2
+// SubVec3 v3 = v1 - v2
 func SubVec3(v1, v2 *Vec3) *Vec3 {
 	return &Vec3{
 		Data: [3]float32{
@@ -92,3 +257,19 @@ func SubVec3(v1, v2 *Vec3) *Vec3 {
 		},
 	}
 }
+
+func NewVec3(x, y, z float32) *Vec3 {
+	return &Vec3{
+		[3]float32{
+			x,
+			y,
+			z,
+		},
+	}
+}
+
+func NewVec3Arr(arr [3]float32) *Vec3 {
+	return &Vec3{
+		Data: arr,
+	}
+}

gglm/vec4.go

@@ -0,0 +1,303 @@
+package gglm
+
+import (
+	"fmt"
+	"math"
+)
+
+var _ Swizzle4 = &Vec4{}
+var _ fmt.Stringer = &Vec4{}
+
+type Vec4 struct {
+	Data [4]float32
+}
+
+func (v *Vec4) X() float32 {
+	return v.Data[0]
+}
+
+func (v *Vec4) Y() float32 {
+	return v.Data[1]
+}
+
+func (v *Vec4) Z() float32 {
+	return v.Data[2]
+}
+
+func (v *Vec4) W() float32 {
+	return v.Data[3]
+}
+
+func (v *Vec4) R() float32 {
+	return v.Data[0]
+}
+
+func (v *Vec4) G() float32 {
+	return v.Data[1]
+}
+
+func (v *Vec4) B() float32 {
+	return v.Data[2]
+}
+
+func (v *Vec4) A() float32 {
+	return v.Data[3]
+}
+
+func (v *Vec4) SetX(f float32) {
+	v.Data[0] = f
+}
+
+func (v *Vec4) SetR(f float32) {
+	v.Data[0] = f
+}
+
+func (v *Vec4) SetY(f float32) {
+	v.Data[1] = f
+}
+
+func (v *Vec4) SetG(f float32) {
+	v.Data[1] = f
+}
+
+func (v *Vec4) SetZ(f float32) {
+	v.Data[2] = f
+}
+
+func (v *Vec4) SetB(f float32) {
+	v.Data[2] = f
+}
+
+func (v *Vec4) SetW(f float32) {
+	v.Data[3] = f
+}
+
+func (v *Vec4) SetA(f float32) {
+	v.Data[3] = f
+}
+
+func (v *Vec4) AddX(x float32) {
+	v.Data[0] += x
+}
+
+func (v *Vec4) AddY(y float32) {
+	v.Data[1] += y
+}
+
+func (v *Vec4) AddZ(z float32) {
+	v.Data[2] += z
+}
+
+func (v *Vec4) AddW(w float32) {
+	v.Data[3] += w
+}
+
+func (v *Vec4) AddR(r float32) {
+	v.Data[0] += r
+}
+
+func (v *Vec4) AddG(g float32) {
+	v.Data[1] += g
+}
+
+func (v *Vec4) AddB(b float32) {
+	v.Data[2] += b
+}
+
+func (v *Vec4) AddA(a float32) {
+	v.Data[3] += a
+}
+
+func (v *Vec4) SetXY(x, y float32) {
+	v.Data[0] = x
+	v.Data[1] = y
+}
+
+func (v *Vec4) AddXY(x, y float32) {
+	v.Data[0] += x
+	v.Data[1] += y
+}
+
+func (v *Vec4) SetRG(r, g float32) {
+	v.Data[0] = r
+	v.Data[1] = g
+}
+
+func (v *Vec4) AddRG(r, g float32) {
+	v.Data[0] += r
+	v.Data[1] += g
+}
+
+func (v *Vec4) SetXYZ(x, y, z float32) {
+	v.Data[0] = x
+	v.Data[1] = y
+	v.Data[2] = z
+}
+
+func (v *Vec4) AddXYZ(x, y, z float32) {
+	v.Data[0] += x
+	v.Data[1] += y
+	v.Data[2] += z
+}
+
+func (v *Vec4) SetRGB(r, g, b float32) {
+	v.Data[0] = r
+	v.Data[1] = g
+	v.Data[2] = b
+}
+
+func (v *Vec4) AddRGB(r, g, b float32) {
+	v.Data[0] += r
+	v.Data[1] += g
+	v.Data[2] += b
+}
+
+func (v *Vec4) SetXYZW(x, y, z, w float32) {
+	v.Data[0] = x
+	v.Data[1] = y
+	v.Data[2] = z
+	v.Data[3] = w
+}
+
+func (v *Vec4) AddXYZW(x, y, z, w float32) {
+	v.Data[0] += x
+	v.Data[1] += y
+	v.Data[2] += z
+	v.Data[3] += w
+}
+
+func (v *Vec4) SetRGBA(r, g, b, a float32) {
+	v.Data[0] = r
+	v.Data[1] = g
+	v.Data[2] = b
+	v.Data[3] = a
+}
+
+func (v *Vec4) AddRGBA(r, g, b, a float32) {
+	v.Data[0] += r
+	v.Data[1] += g
+	v.Data[2] += b
+	v.Data[3] += a
+}
+
+func (v *Vec4) String() string {
+	return fmt.Sprintf("(%f, %f, %f, %f)", v.X(), v.Y(), v.Z(), v.W())
+}
+
+// Scale v *= x (element wise multiplication)
+func (v *Vec4) Scale(x float32) *Vec4 {
+	v.Data[0] *= x
+	v.Data[1] *= x
+	v.Data[2] *= x
+	v.Data[3] *= x
+	return v
+}
+
+// ScaleVec v *= v2 (element wise multiplication)
+func (v *Vec4) ScaleVec(v2 *Vec4) *Vec4 {
+	v.Data[0] *= v2.X()
+	v.Data[1] *= v2.Y()
+	v.Data[2] *= v2.Z()
+	v.Data[3] *= v2.W()
+	return v
+}
+
+// ScaleArr v *= arr (element wise multiplication)
+func (v *Vec4) ScaleArr(arr [4]float32) *Vec4 {
+	v.Data[0] *= arr[0]
+	v.Data[1] *= arr[1]
+	v.Data[2] *= arr[2]
+	v.Data[3] *= arr[3]
+	return v
+}
+
+func (v *Vec4) Add(v2 *Vec4) *Vec4 {
+	v.Data[0] += v2.X()
+	v.Data[1] += v2.Y()
+	v.Data[2] += v2.Z()
+	v.Data[3] += v2.W()
+	return v
+}
+
+// SubVec4 v -= v2
+func (v *Vec4) Sub(v2 *Vec4) *Vec4 {
+	v.Data[0] -= v2.X()
+	v.Data[1] -= v2.Y()
+	v.Data[2] -= v2.Z()
+	v.Data[3] -= v2.W()
+	return v
+}
+
+// Mag returns the magnitude of the vector
+func (v *Vec4) Mag() float32 {
+	return float32(math.Sqrt(float64(v.X()*v.X() + v.Y()*v.Y() + v.Z()*v.Z() + v.W()*v.W())))
+}
+
+// Mag returns the squared magnitude of the vector
+func (v *Vec4) SqrMag() float32 {
+	return v.X()*v.X() + v.Y()*v.Y() + v.Z()*v.Z() + v.Z()*v.Z()
+}
+
+func (v *Vec4) Eq(v2 *Vec4) bool {
+	return v.Data == v2.Data
+}
+
+func (v *Vec4) Set(x, y, z, w float32) {
+	v.Data[0] = x
+	v.Data[1] = y
+	v.Data[2] = z
+	v.Data[3] = w
+}
+
+func (v *Vec4) Normalize() {
+	mag := float32(math.Sqrt(float64(v.Data[0]*v.Data[0] + v.Data[1]*v.Data[1] + v.Data[2]*v.Data[2] + v.Data[3]*v.Data[3])))
+	v.Data[0] /= mag
+	v.Data[1] /= mag
+	v.Data[2] /= mag
+	v.Data[3] /= mag
+}
+
+func (v *Vec4) Clone() *Vec4 {
+	return &Vec4{Data: v.Data}
+}
+
+// AddVec4 v3 = v1 + v2
+func AddVec4(v1, v2 *Vec4) *Vec4 {
+	return &Vec4{
+		Data: [4]float32{
+			v1.X() + v2.X(),
+			v1.Y() + v2.Y(),
+			v1.Z() + v2.Z(),
+			v1.W() + v2.W(),
+		},
+	}
+}
+
+// SubVec4 v3 = v1 - v2
+func SubVec4(v1, v2 *Vec4) *Vec4 {
+	return &Vec4{
+		Data: [4]float32{
+			v1.X() - v2.X(),
+			v1.Y() - v2.Y(),
+			v1.Z() - v2.Z(),
+			v1.W() - v2.W(),
+		},
+	}
+}
+
+func NewVec4(x, y, z, w float32) *Vec4 {
+	return &Vec4{
+		[4]float32{
+			x,
+			y,
+			z,
+			w,
+		},
+	}
+}
+
+func NewVec4Arr(arr [4]float32) *Vec4 {
+	return &Vec4{
+		Data: arr,
+	}
+}

gglm/vec_test.go

@@ -0,0 +1,423 @@
+package gglm_test
+
+import (
+	"testing"
+
+	"github.com/bloeys/gglm/gglm"
+)
+
+func TestVecSwizzleGet(t *testing.T) {
+
+	//Vec2
+	v2 := gglm.NewVec2(1, 2)
+	var ans2X float32 = 1
+	var ans2Y float32 = 2
+
+	if v2.X() != ans2X {
+		t.Errorf("Got: %v; Expected: %v", v2.X(), ans2X)
+	}
+
+	if v2.Y() != ans2Y {
+		t.Errorf("Got: %v; Expected: %v", v2.Y(), ans2Y)
+	}
+
+	if v2.R() != ans2X {
+		t.Errorf("Got: %v; Expected: %v", v2.R(), ans2X)
+	}
+
+	if v2.G() != ans2Y {
+		t.Errorf("Got: %v; Expected: %v", v2.G(), ans2Y)
+	}
+
+	//Vec3
+	v3 := gglm.NewVec3(1, 2, 3)
+	var ans3X float32 = 1
+	var ans3Y float32 = 2
+	var ans3Z float32 = 3
+
+	if v3.X() != ans3X {
+		t.Errorf("Got: %v; Expected: %v", v3.X(), ans3X)
+	}
+
+	if v3.Y() != ans3Y {
+		t.Errorf("Got: %v; Expected: %v", v3.Y(), ans3Y)
+	}
+
+	if v3.Z() != ans3Z {
+		t.Errorf("Got: %v; Expected: %v", v3.Z(), ans3Z)
+	}
+
+	if v3.R() != ans3X {
+		t.Errorf("Got: %v; Expected: %v", v3.R(), ans3X)
+	}
+
+	if v3.G() != ans3Y {
+		t.Errorf("Got: %v; Expected: %v", v3.G(), ans3Y)
+	}
+
+	if v3.B() != ans3Z {
+		t.Errorf("Got: %v; Expected: %v", v3.B(), ans3Z)
+	}
+
+	//Vec4
+	v4 := gglm.NewVec4(1, 2, 3, 4)
+	var ans4X float32 = 1
+	var ans4Y float32 = 2
+	var ans4Z float32 = 3
+	var ans4W float32 = 4
+
+	if v4.X() != ans4X {
+		t.Errorf("Got: %v; Expected: %v", v4.X(), ans4X)
+	}
+
+	if v4.Y() != ans4Y {
+		t.Errorf("Got: %v; Expected: %v", v4.Y(), ans4Y)
+	}
+
+	if v4.Z() != ans4Z {
+		t.Errorf("Got: %v; Expected: %v", v4.Z(), ans4Z)
+	}
+
+	if v4.W() != ans4W {
+		t.Errorf("Got: %v; Expected: %v", v4.W(), ans4W)
+	}
+
+	if v4.R() != ans4X {
+		t.Errorf("Got: %v; Expected: %v", v4.R(), ans4X)
+	}
+
+	if v4.G() != ans4Y {
+		t.Errorf("Got: %v; Expected: %v", v4.G(), ans4Y)
+	}
+
+	if v4.B() != ans4Z {
+		t.Errorf("Got: %v; Expected: %v", v4.B(), ans4Z)
+	}
+
+	if v4.A() != ans4W {
+		t.Errorf("Got: %v; Expected: %v", v4.A(), ans4W)
+	}
+}
+
+func TestVecSwizzleSet(t *testing.T) {
+
+	//Vec2
+	v2 := gglm.NewVec2(1, 1)
+	ans2 := gglm.NewVec2(1, 2)
+
+	v2.SetX(1)
+	v2.SetY(2)
+	if !v2.Eq(ans2) {
+		t.Errorf("Got: %v; Expected: %v", v2.String(), ans2.String())
+	}
+
+	v2 = gglm.NewVec2(1, 1)
+	ans2 = gglm.NewVec2(11, 22)
+
+	v2.SetR(11)
+	v2.SetG(22)
+	if !v2.Eq(ans2) {
+		t.Errorf("Got: %v; Expected: %v", v2.String(), ans2.String())
+	}
+
+	v2 = gglm.NewVec2(1, 1)
+	ans2 = gglm.NewVec2(1, 2)
+
+	v2.SetXY(1, 2)
+	if !v2.Eq(ans2) {
+		t.Errorf("Got: %v; Expected: %v", v2.String(), ans2.String())
+	}
+
+	v2 = gglm.NewVec2(1, 1)
+	ans2 = gglm.NewVec2(11, 22)
+
+	v2.SetRG(11, 22)
+	if !v2.Eq(ans2) {
+		t.Errorf("Got: %v; Expected: %v", v2.String(), ans2.String())
+	}
+
+	//Vec3
+	v3 := gglm.NewVec3(1, 1, 1)
+	ans3 := gglm.NewVec3(1, 2, 3)
+
+	v3.SetX(1)
+	v3.SetY(2)
+	v3.SetZ(3)
+
+	if !v3.Eq(ans3) {
+		t.Errorf("Got: %v; Expected: %v", v3.String(), ans3.String())
+	}
+
+	v3 = gglm.NewVec3(1, 1, 1)
+	ans3 = gglm.NewVec3(11, 22, 33)
+
+	v3.SetR(11)
+	v3.SetG(22)
+	v3.SetB(33)
+
+	if !v3.Eq(ans3) {
+		t.Errorf("Got: %v; Expected: %v", v3.String(), ans3.String())
+	}
+
+	v3 = gglm.NewVec3(1, 1, 1)
+	ans3 = gglm.NewVec3(1, 2, 1)
+
+	v3.SetXY(1, 2)
+	if !v3.Eq(ans3) {
+		t.Errorf("Got: %v; Expected: %v", v3.String(), ans3.String())
+	}
+
+	v3 = gglm.NewVec3(1, 1, 1)
+	ans3 = gglm.NewVec3(1, 2, 1)
+
+	v3.SetRG(1, 2)
+	if !v3.Eq(ans3) {
+		t.Errorf("Got: %v; Expected: %v", v3.String(), ans3.String())
+	}
+
+	v3 = gglm.NewVec3(1, 1, 1)
+	ans3 = gglm.NewVec3(1, 2, 3)
+
+	v3.SetXYZ(1, 2, 3)
+	if !v3.Eq(ans3) {
+		t.Errorf("Got: %v; Expected: %v", v3.String(), ans3.String())
+	}
+
+	v3 = gglm.NewVec3(1, 1, 1)
+	ans3 = gglm.NewVec3(1, 2, 3)
+
+	v3.SetRGB(1, 2, 3)
+	if !v3.Eq(ans3) {
+		t.Errorf("Got: %v; Expected: %v", v3.String(), ans3.String())
+	}
+
+	// Test AngleVec3
+	v3 = gglm.NewVec3(1, 0, 0)
+	v32 := gglm.NewVec3(1, 0, 0)
+
+	angleV3 := gglm.AngleVec3(v3, v32) * gglm.Rad2Deg
+	if angleV3 != 0 {
+		t.Errorf("Got: %v; Expected: %v", v3.String(), 0)
+	}
+
+	v32.SetXY(0, 1)
+	angleV3 = gglm.AngleVec3(v3, v32) * gglm.Rad2Deg
+	if angleV3 != 90 {
+		t.Errorf("Got: %v; Expected: %v", v3.String(), 0)
+	}
+
+	// Test rot by quat
+	v32.SetXY(1, 0)
+	v32.RotByQuat(gglm.NewQuatAngleAxis(90*gglm.Deg2Rad, gglm.NewVec3(0, 1, 0)))
+	angleV3 = gglm.AngleVec3(v3, v32) * gglm.Rad2Deg
+	if angleV3 != 90 {
+		t.Errorf("Got: %v; Expected: %v", v3.String(), 0)
+	}
+
+	//Vec4
+	v4 := gglm.NewVec4(1, 1, 1, 1)
+	ans4 := gglm.NewVec4(1, 2, 3, 4)
+
+	v4.SetX(1)
+	v4.SetY(2)
+	v4.SetZ(3)
+	v4.SetW(4)
+
+	if !v4.Eq(ans4) {
+		t.Errorf("Got: %v; Expected: %v", v4.String(), ans4.String())
+	}
+
+	v4 = gglm.NewVec4(1, 1, 1, 1)
+	ans4 = gglm.NewVec4(11, 22, 33, 44)
+
+	v4.SetR(11)
+	v4.SetG(22)
+	v4.SetB(33)
+	v4.SetA(44)
+
+	if !v4.Eq(ans4) {
+		t.Errorf("Got: %v; Expected: %v", v4.String(), ans4.String())
+	}
+
+	v4 = gglm.NewVec4(1, 1, 1, 1)
+	ans4 = gglm.NewVec4(1, 2, 1, 1)
+
+	v4.SetXY(1, 2)
+	if !v4.Eq(ans4) {
+		t.Errorf("Got: %v; Expected: %v", v4.String(), ans4.String())
+	}
+
+	v4 = gglm.NewVec4(1, 1, 1, 1)
+	ans4 = gglm.NewVec4(1, 2, 1, 1)
+
+	v4.SetRG(1, 2)
+	if !v4.Eq(ans4) {
+		t.Errorf("Got: %v; Expected: %v", v4.String(), ans4.String())
+	}
+
+	v4 = gglm.NewVec4(1, 1, 1, 1)
+	ans4 = gglm.NewVec4(1, 2, 3, 1)
+
+	v4.SetXYZ(1, 2, 3)
+	if !v4.Eq(ans4) {
+		t.Errorf("Got: %v; Expected: %v", v4.String(), ans4.String())
+	}
+
+	v4 = gglm.NewVec4(1, 1, 1, 1)
+	ans4 = gglm.NewVec4(1, 2, 3, 1)
+
+	v4.SetRGB(1, 2, 3)
+	if !v4.Eq(ans4) {
+		t.Errorf("Got: %v; Expected: %v", v4.String(), ans4.String())
+	}
+
+	v4 = gglm.NewVec4(1, 1, 1, 1)
+	ans4 = gglm.NewVec4(1, 2, 3, 4)
+
+	v4.SetXYZW(1, 2, 3, 4)
+	if !v4.Eq(ans4) {
+		t.Errorf("Got: %v; Expected: %v", v4.String(), ans4.String())
+	}
+
+	v4 = gglm.NewVec4(1, 1, 1, 1)
+	ans4 = gglm.NewVec4(1, 2, 3, 4)
+
+	v4.SetRGBA(1, 2, 3, 4)
+	if !v4.Eq(ans4) {
+		t.Errorf("Got: %v; Expected: %v", v4.String(), ans4.String())
+	}
+}
+
+func TestVecSwizzleAdd(t *testing.T) {
+
+	//Vec2
+	v2 := gglm.NewVec2(1, 1)
+	ans2 := gglm.NewVec2(2, 3)
+	v2.AddX(1)
+	v2.AddY(2)
+	if !v2.Eq(ans2) {
+		t.Errorf("Got: %v; Expected: %v", v2.String(), ans2.String())
+	}
+
+	v2 = gglm.NewVec2(1, 1)
+	v2.AddR(1)
+	v2.AddG(2)
+	if !v2.Eq(ans2) {
+		t.Errorf("Got: %v; Expected: %v", v2.String(), ans2.String())
+	}
+
+	v2 = gglm.NewVec2(1, 1)
+	v2.AddXY(1, 2)
+	if !v2.Eq(ans2) {
+		t.Errorf("Got: %v; Expected: %v", v2.String(), ans2.String())
+	}
+
+	v2 = gglm.NewVec2(1, 1)
+	v2.AddRG(1, 2)
+	if !v2.Eq(ans2) {
+		t.Errorf("Got: %v; Expected: %v", v2.String(), ans2.String())
+	}
+
+	//Vec3
+	v3 := gglm.NewVec3(1, 1, 1)
+	ans3 := gglm.NewVec3(2, 3, 4)
+	v3.AddX(1)
+	v3.AddY(2)
+	v3.AddZ(3)
+	if !v3.Eq(ans3) {
+		t.Errorf("Got: %v; Expected: %v", v3.String(), ans3.String())
+	}
+
+	v3 = gglm.NewVec3(1, 1, 1)
+	v3.AddR(1)
+	v3.AddG(2)
+	v3.AddB(3)
+	if !v3.Eq(ans3) {
+		t.Errorf("Got: %v; Expected: %v", v3.String(), ans3.String())
+	}
+
+	v3 = gglm.NewVec3(1, 1, 1)
+	ans3 = gglm.NewVec3(2, 3, 1)
+	v3.AddXY(1, 2)
+	if !v3.Eq(ans3) {
+		t.Errorf("Got: %v; Expected: %v", v3.String(), ans3.String())
+	}
+
+	v3 = gglm.NewVec3(1, 1, 1)
+	v3.AddRG(1, 2)
+	if !v3.Eq(ans3) {
+		t.Errorf("Got: %v; Expected: %v", v3.String(), ans3.String())
+	}
+
+	v3 = gglm.NewVec3(1, 1, 1)
+	ans3 = gglm.NewVec3(2, 3, 4)
+	v3.AddXYZ(1, 2, 3)
+	if !v3.Eq(ans3) {
+		t.Errorf("Got: %v; Expected: %v", v3.String(), ans3.String())
+	}
+
+	v3 = gglm.NewVec3(1, 1, 1)
+	v3.AddRGB(1, 2, 3)
+	if !v3.Eq(ans3) {
+		t.Errorf("Got: %v; Expected: %v", v3.String(), ans3.String())
+	}
+
+	//Vec4
+	v4 := gglm.NewVec4(1, 1, 1, 1)
+	ans4 := gglm.NewVec4(2, 3, 4, 5)
+	v4.AddX(1)
+	v4.AddY(2)
+	v4.AddZ(3)
+	v4.AddW(4)
+	if !v4.Eq(ans4) {
+		t.Errorf("Got: %v; Expected: %v", v4.String(), ans4.String())
+	}
+
+	v4 = gglm.NewVec4(1, 1, 1, 1)
+	v4.AddR(1)
+	v4.AddG(2)
+	v4.AddB(3)
+	v4.AddA(4)
+	if !v4.Eq(ans4) {
+		t.Errorf("Got: %v; Expected: %v", v4.String(), ans4.String())
+	}
+
+	v4 = gglm.NewVec4(1, 1, 1, 1)
+	ans4 = gglm.NewVec4(2, 3, 1, 1)
+	v4.AddXY(1, 2)
+	if !v4.Eq(ans4) {
+		t.Errorf("Got: %v; Expected: %v", v4.String(), ans4.String())
+	}
+
+	v4 = gglm.NewVec4(1, 1, 1, 1)
+	v4.AddRG(1, 2)
+	if !v4.Eq(ans4) {
+		t.Errorf("Got: %v; Expected: %v", v4.String(), ans4.String())
+	}
+
+	v4 = gglm.NewVec4(1, 1, 1, 1)
+	ans4 = gglm.NewVec4(2, 3, 4, 1)
+	v4.AddXYZ(1, 2, 3)
+	if !v4.Eq(ans4) {
+		t.Errorf("Got: %v; Expected: %v", v4.String(), ans4.String())
+	}
+
+	v4 = gglm.NewVec4(1, 1, 1, 1)
+	v4.AddRGB(1, 2, 3)
+	if !v4.Eq(ans4) {
+		t.Errorf("Got: %v; Expected: %v", v4.String(), ans4.String())
+	}
+
+	v4 = gglm.NewVec4(1, 1, 1, 1)
+	ans4 = gglm.NewVec4(2, 3, 4, 5)
+	v4.AddXYZW(1, 2, 3, 4)
+	if !v4.Eq(ans4) {
+		t.Errorf("Got: %v; Expected: %v", v4.String(), ans4.String())
+	}
+
+	v4 = gglm.NewVec4(1, 1, 1, 1)
+	v4.AddRGBA(1, 2, 3, 4)
+	if !v4.Eq(ans4) {
+		t.Errorf("Got: %v; Expected: %v", v4.String(), ans4.String())
+	}
+}

main.go

@@ -1,23 +1,27 @@
 package main
 
-import "github.com/bloeys/gglm/gglm"
+import (
+	"fmt"
+
+	"github.com/bloeys/gglm/gglm"
+)
 
 func main() {
 
-	//Mat3
+	// Mat3
 	m1 := &gglm.Mat3{
-		Data: [9]float32{
-			1, 2, 3,
-			4, 5, 6,
-			7, 8, 9,
+		Data: [3][3]float32{
+			{1, 4, 7},
+			{2, 5, 8},
+			{3, 6, 9},
 		},
 	}
 
 	m2 := &gglm.Mat3{
-		Data: [9]float32{
-			1, 2, 3,
-			1, 2, 3,
-			1, 2, 3,
+		Data: [3][3]float32{
+			{1, 1, 1},
+			{2, 2, 2},
+			{3, 3, 3},
 		},
 	}
 
@@ -26,22 +30,22 @@ func main() {
 	println(m1.String())
 	println(m3.String())
 
-	//Mat4
+	// Mat4
 	m4 := &gglm.Mat4{
-		Data: [16]float32{
-			1, 2, 3, 4,
-			5, 6, 7, 8,
-			9, 10, 11, 12,
-			13, 14, 15, 16,
+		Data: [4][4]float32{
+			{1, 5, 9, 13},
+			{2, 6, 10, 14},
+			{3, 7, 11, 15},
+			{4, 8, 12, 16},
 		},
 	}
 
 	m5 := &gglm.Mat4{
-		Data: [16]float32{
-			1, 2, 3, 4,
-			1, 2, 3, 4,
-			1, 2, 3, 4,
-			1, 2, 3, 4,
+		Data: [4][4]float32{
+			{1, 2, 3, 4},
+			{1, 2, 3, 4},
+			{1, 2, 3, 4},
+			{1, 2, 3, 4},
 		},
 	}
 
@@ -49,4 +53,152 @@ func main() {
 	m4.Mul(m5)
 	println(m4.String())
 	println(m6.String())
+	println(m4.Eq(m6))
+
+	// Vec2
+	v1 := &gglm.Vec2{Data: [2]float32{1, 2}}
+	v2 := &gglm.Vec2{Data: [2]float32{3, 4}}
+	println(gglm.DistVec2(v1, v2))
+	println(gglm.SqrDistVec2(v2, v1))
+	println(v1.Eq(v2))
+	v2.Set(1, 2)
+	println(v1.Eq(v2))
+
+	v1.AddXY(v2.X(), v2.Y())
+	println(v1.String())
+
+	println("V1: " + v1.String())
+	v1.Normalize()
+	println("V1 Normal: " + v1.String())
+
+	// Vec3
+	v3 := &gglm.Vec3{Data: [3]float32{1, 2, 3}}
+	v4 := &gglm.Vec3{Data: [3]float32{4, 5, 6}}
+	println(gglm.DistVec3(v3, v4))
+	println(gglm.SqrDistVec3(v4, v3))
+
+	println(v3.Eq(v4))
+	v4.Set(1, 2, 3)
+	println(v3.Eq(v4))
+
+	println(gglm.DotVec3(v3, v4))
+	println(gglm.Cross(v3, v4).String())
+
+	println("V3: " + v3.String())
+	v3.Normalize()
+	println("V3 Normal: " + v3.String())
+
+	// Vec4
+	v5 := &gglm.Vec4{Data: [4]float32{1, 2, 3, 4}}
+	v6 := &gglm.Vec4{Data: [4]float32{5, 6, 7, 8}}
+	println(gglm.DistVec4(v5, v6))
+	println(gglm.SqrDistVec4(v5, v6))
+
+	println(v5.Eq(v6))
+	v6.Set(1, 2, 3, 4)
+	println(v5.Eq(v6))
+
+	println(gglm.DotVec4(v5, v6))
+
+	v5.Add(v6)
+	v5.AddXYZW(v6.X(), v6.Y(), v6.Z(), v6.W())
+	println(v6.String())
+
+	println("V6: " + v6.String())
+	v6.Normalize()
+	println("V6 Normal: " + v6.String())
+
+	// Mat2Vec2
+	mat2A := gglm.Mat2{
+		Data: [2][2]float32{
+			{1, 3},
+			{2, 4},
+		},
+	}
+
+	vec2A := gglm.Vec2{Data: [2]float32{1, 2}}
+	println(gglm.MulMat2Vec2(&mat2A, &vec2A).String())
+
+	// Mat3Vec3
+	mat3A := gglm.Mat3{
+		Data: [3][3]float32{
+			{1, 4, 7},
+			{2, 5, 8},
+			{3, 6, 9},
+		},
+	}
+
+	vec3A := gglm.Vec3{Data: [3]float32{1, 2, 3}}
+	mm3v3 := gglm.MulMat3Vec3(&mat3A, &vec3A)
+	println(mm3v3.String())
+
+	// ReflectVec2
+	vec2B := &gglm.Vec2{Data: [2]float32{4, 5}}
+	normA := &gglm.Vec2{Data: [2]float32{0, 1}}
+	rVec2A := gglm.ReflectVec2(vec2B, normA)
+	println(rVec2A.String())
+
+	// Quaternion
+	vRot := &gglm.Vec3{Data: [3]float32{60, 30, 20}}
+	q := gglm.NewQuatEuler(vRot.AsRad())
+	println("\n" + vRot.AsRad().String())
+	println(q.String(), "\n", q.Mag())
+
+	q = gglm.NewQuatAngleAxis(60*gglm.Deg2Rad, vRot.Normalize())
+	println("\n" + vRot.Normalize().String())
+	println(q.String())
+
+	// Transform
+	translationMat := gglm.NewTranslationMat(&gglm.Vec3{Data: [3]float32{1, 2, 3}})
+	rotMat := gglm.NewRotMat(gglm.NewQuatEuler(gglm.NewVec3(60, 30, 20).AsRad()))
+	scaleMat := gglm.NewScaleMat(gglm.NewVec3(1, 1, 1))
+
+	modelMat := gglm.NewTrMatId()
+	modelMat.Mul(translationMat.Mul(rotMat.Mul(scaleMat)))
+
+	println("\n\n\n", modelMat.String())
+
+	// Clone Vec2
+	v2Orig := gglm.Vec2{Data: [2]float32{1, 2}}
+	v2Clone := v2Orig.Clone()
+	v2Clone.SetX(99)
+	println("\n\n", v2Orig.String(), "; ", v2Clone.String())
+
+	// Clone TrMat
+	trMatOrig := gglm.NewTranslationMat(gglm.NewVec3(1, 2, 3))
+	trMatClone := trMatOrig.Clone()
+	trMatClone.Scale(gglm.NewVec3(2, 2, 2))
+	trMatClone.Translate(gglm.NewVec3(9, 0, 0))
+	println("\n\n", trMatOrig.String(), "; ", trMatClone.String())
+
+	// Quat geo
+	q1 := gglm.NewQuatEuler(gglm.NewVec3(180, 0, 0).AsRad())
+	q2 := gglm.NewQuatEuler(gglm.NewVec3(0, 180, 0).AsRad())
+	println(gglm.AngleQuat(q1, q2) * gglm.Rad2Deg)
+
+	// LookAt
+	camPos := gglm.NewVec3(0, 0, 3)
+	worldUp := gglm.NewVec3(0, 1, 0)
+	targetPos := gglm.NewVec3(0, 0, 0)
+	viewMat := gglm.LookAtRH(camPos, targetPos, worldUp)
+	println(viewMat.String())
+
+	// Mat2Col
+	mc := gglm.NewMat2Id()
+	println("===============================")
+	println(mc.String())
+
+	mc.Data = [2][2]float32{
+		{1, 3},
+		{2, 4},
+	}
+	println(mc.String())
+	fmt.Printf("Arr: %v", mc.Data)
+
+	mc2 := gglm.Mat2{Data: [2][2]float32{
+		{1, 3},
+		{2, 4},
+	}}
+
+	println(mc2.Mul(mc).String())
 }

main_test.go

@@ -1,27 +0,0 @@
-package main
-
-import (
-	"testing"
-
-	"github.com/bloeys/gglm/gglm"
-)
-
-func BenchmarkVec3Add(b *testing.B) {
-
-	v1 := gglm.NewMat3Id()
-	v2 := gglm.NewMat3Id()
-
-	for i := 0; i < b.N; i++ {
-		v1.Mul(v2)
-	}
-}
-
-func BenchmarkVec3Add2(b *testing.B) {
-
-	v1 := gglm.NewMat3Id()
-	v2 := gglm.NewMat3Id()
-
-	for i := 0; i < b.N; i++ {
-		v1.Mul(v2)
-	}
-}