spatial/{r2,r3}: add Cos, Unit

spatial/r3/vector_test.go

@@ -4,7 +4,12 @@
 
 package r3
 
-import "testing"
+import (
+	"math"
+	"testing"
+
+	"gonum.org/v1/gonum/floats"
+)
 
 func TestAdd(t *testing.T) {
 	for _, test := range []struct {
@@ -176,3 +181,72 @@ func TestNorm2(t *testing.T) {
 		})
 	}
 }
+
+func TestUnit(t *testing.T) {
+	for _, test := range []struct {
+		v, want Vec
+	}{
+		{Vec{}, Vec{math.NaN(), math.NaN(), math.NaN()}},
+		{Vec{1, 0, 0}, Vec{1, 0, 0}},
+		{Vec{0, 1, 0}, Vec{0, 1, 0}},
+		{Vec{0, 0, 1}, Vec{0, 0, 1}},
+		{Vec{1, 1, 1}, Vec{1. / math.Sqrt(3), 1. / math.Sqrt(3), 1. / math.Sqrt(3)}},
+		{Vec{1, 1e-16, 1e-32}, Vec{1, 1e-16, 1e-32}},
+	} {
+		t.Run("", func(t *testing.T) {
+			got := Unit(test.v)
+			if !vecEqual(got, test.want) {
+				t.Fatalf(
+					"Normalize(%v) = %v, want %v",
+					test.v, got, test.want,
+				)
+			}
+			if test.v == (Vec{}) {
+				return
+			}
+			if n, want := Norm(got), 1.0; n != want {
+				t.Fatalf("|%v| = %v, want 1", got, n)
+			}
+		})
+	}
+}
+
+func TestCos(t *testing.T) {
+	for _, test := range []struct {
+		v1, v2 Vec
+		want   float64
+	}{
+		{Vec{1, 1, 1}, Vec{1, 1, 1}, 1},
+		{Vec{1, 1, 1}, Vec{-1, -1, -1}, -1},
+		{Vec{1, 1, 1}, Vec{1, -1, 1}, 1.0 / 3},
+		{Vec{1, 0, 0}, Vec{1, 0, 0}, 1},
+		{Vec{1, 0, 0}, Vec{0, 1, 0}, 0},
+		{Vec{1, 0, 0}, Vec{0, 1, 1}, 0},
+		{Vec{1, 0, 0}, Vec{-1, 0, 0}, -1},
+	} {
+		t.Run("", func(t *testing.T) {
+			tol := 1e-14
+			got := Cos(test.v1, test.v2)
+			if !floats.EqualWithinAbs(got, test.want, tol) {
+				t.Fatalf("cos(%v, %v)= %v, want %v",
+					test.v1, test.v2, got, test.want,
+				)
+			}
+		})
+	}
+}
+
+func vecIsNaN(v Vec) bool {
+	return math.IsNaN(v.X) && math.IsNaN(v.Y) && math.IsNaN(v.Z)
+}
+
+func vecIsNaNAny(v Vec) bool {
+	return math.IsNaN(v.X) || math.IsNaN(v.Y) || math.IsNaN(v.Z)
+}
+
+func vecEqual(a, b Vec) bool {
+	if vecIsNaNAny(a) || vecIsNaNAny(b) {
+		return vecIsNaN(a) && vecIsNaN(b)
+	}
+	return a == b
+}