Implement RotByQuat and AngleVec3 and their tests

gglm/geometric.go

@@ -111,6 +111,11 @@ func ReflectVec3(v, n *Vec3) *Vec3 {
 	}
 }
 
+// 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/vec3.go

@@ -186,6 +186,25 @@ func (v *Vec3) Normalize() *Vec3 {
 	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}
 }

gglm/vec_test.go

@@ -191,6 +191,29 @@ func TestVecSwizzleSet(t *testing.T) {
 		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)