Implement RotByQuat and AngleVec3 and their tests

gglm/geometric.go

@@ -28,14 +28,14 @@ func Cross(v1, v2 *Vec3) *Vec3 {
 	}
 }
 
-//DistVec2 returns euclidean distance between v1 and v2
+// 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
+// DistVec3 returns euclidean distance between v1 and v2
 func DistVec3(v1, v2 *Vec3) float32 {
 	x := v1.X() - v2.X()
 	y := v1.Y() - v2.Y()
@@ -43,7 +43,7 @@ func DistVec3(v1, v2 *Vec3) float32 {
 	return float32(math.Sqrt(float64(x*x + y*y + z*z)))
 }
 
-//DistVec4 returns euclidean distance between v1 and v2
+// DistVec4 returns euclidean distance between v1 and v2
 func DistVec4(v1, v2 *Vec4) float32 {
 
 	//Using X() etc won't let the function inline
@@ -54,14 +54,14 @@ func DistVec4(v1, v2 *Vec4) float32 {
 	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)
+// 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)
+// 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()
@@ -69,7 +69,7 @@ func SqrDistVec3(v1, v2 *Vec3) float32 {
 	return x*x + y*y + z*z
 }
 
-//DistVec4 returns the squared euclidean distance between v1 and v2 (avoids a sqrt)
+// 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]
@@ -78,9 +78,9 @@ func SqrDistVec4(v1, v2 *Vec4) float32 {
 	return x*x + y*y + z*z + w*w
 }
 
-//ReflectVec2 returns the reflected vector of the incoming vector 'v', and the surface normal 'n'.
+// 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
+// Note: n must be normalized or you will get wrong results
 func ReflectVec2(v, n *Vec2) *Vec2 {
 
 	//reflectedVec = v − 2*dot(v, norm)*norm
@@ -94,9 +94,9 @@ func ReflectVec2(v, n *Vec2) *Vec2 {
 	}
 }
 
-//ReflectVec3 returns the reflected vector of the incoming vector 'v', and the surface normal 'n'.
+// 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
+// Note: n must be normalized or you will get wrong results
 func ReflectVec3(v, n *Vec3) *Vec3 {
 
 	//reflectedVec = v − 2*dot(v, norm)*norm
@@ -111,7 +111,12 @@ func ReflectVec3(v, n *Vec3) *Vec3 {
 	}
 }
 
-//AngleQuat returns the angle between the two quaternions in radians
+// 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

@@ -132,7 +132,7 @@ func (v *Vec3) String() string {
 	return fmt.Sprintf("(%f, %f, %f)", v.X(), v.Y(), v.Z())
 }
 
-//Scale v *= x (element wise multiplication)
+// Scale v *= x (element wise multiplication)
 func (v *Vec3) Scale(x float32) *Vec3 {
 	v.Data[0] *= x
 	v.Data[1] *= x
@@ -148,7 +148,7 @@ func (v *Vec3) Add(v2 *Vec3) *Vec3 {
 	return v
 }
 
-//SubVec3 v -= v2
+// SubVec3 v -= v2
 func (v *Vec3) Sub(v2 *Vec3) *Vec3 {
 	v.Data[0] -= v2.X()
 	v.Data[1] -= v2.Y()
@@ -156,12 +156,12 @@ func (v *Vec3) Sub(v2 *Vec3) *Vec3 {
 	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()
 }
@@ -176,7 +176,7 @@ func (v *Vec3) Set(x, y, z float32) {
 	v.Data[2] = z
 }
 
-//Normalize normalizes this vector and returns it (doesn't copy)
+// 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
@@ -186,11 +186,30 @@ 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}
 }
 
-//AsRad returns a new vector with all values converted to Radians (i.e. multiplied by gglm.Deg2Rad)
+// 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{
@@ -201,7 +220,7 @@ func (v *Vec3) AsRad() *Vec3 {
 	}
 }
 
-//AddVec3 v3 = v1 + v2
+// AddVec3 v3 = v1 + v2
 func AddVec3(v1, v2 *Vec3) *Vec3 {
 	return &Vec3{
 		Data: [3]float32{
@@ -212,7 +231,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{

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)