spatial/r2: implement Rotate

Fixes gonum/gonum#1513.

spatial/r2/vector.go

@@ -42,6 +42,11 @@ func (p Vec) Cross(q Vec) float64 {
 	return p.X*q.Y - p.Y*q.X
 }
 
+// Rotate returns a new vector, rotated by alpha around the provided point, q.
+func (p Vec) Rotate(alpha float64, q Vec) Vec {
+	return NewRotation(alpha, q).Rotate(p)
+}
+
 // Norm returns the Euclidean norm of p
 //  |p| = sqrt(p_x^2 + p_y^2).
 func Norm(p Vec) float64 {
@@ -72,3 +77,34 @@ func Cos(p, q Vec) float64 {
 type Box struct {
 	Min, Max Vec
 }
+
+// Rotation describes a rotation in 2D.
+type Rotation struct {
+	sin, cos float64
+	p        Vec
+}
+
+// NewRotation creates a rotation by alpha, around p.
+func NewRotation(alpha float64, p Vec) Rotation {
+	if alpha == 0 {
+		return Rotation{sin: 0, cos: 1, p: p}
+	}
+	sin, cos := math.Sincos(alpha)
+	return Rotation{sin: sin, cos: cos, p: p}
+}
+
+// Rotate returns the rotated vector according to the definition of rot.
+func (r Rotation) Rotate(p Vec) Vec {
+	if r.isIdentity() {
+		return p
+	}
+	o := p.Sub(r.p)
+	return Vec{
+		X: (o.X*r.cos - o.Y*r.sin),
+		Y: (o.X*r.sin + o.Y*r.cos),
+	}.Add(r.p)
+}
+
+func (r Rotation) isIdentity() bool {
+	return r.sin == 0 && r.cos == 1
+}

spatial/r2/vector_test.go

@@ -240,6 +240,31 @@ func TestCos(t *testing.T) {
 	}
 }
 
+func TestRotate(t *testing.T) {
+	for _, test := range []struct {
+		v, q  Vec
+		alpha float64
+		want  Vec
+	}{
+		{Vec{1, 0}, Vec{0, 0}, math.Pi / 2, Vec{0, 1}},
+		{Vec{1, 0}, Vec{0, 0}, 3 * math.Pi / 2, Vec{0, -1}},
+		{Vec{1, 0}, Vec{0, 1}, 0, Vec{1, 0}},
+		{Vec{1, 0}, Vec{0, 1}, 2 * math.Pi, Vec{1, 0}},
+		{Vec{1, 1}, Vec{0, 0}, math.Pi, Vec{-1, -1}},
+		{Vec{2, 2}, Vec{1, 1}, math.Pi / 2, Vec{0, 2}},
+		{Vec{2, 2}, Vec{1, 1}, math.Pi, Vec{0, 0}},
+		{Vec{2, 2}, Vec{2, 0}, math.Pi, Vec{2, -2}},
+	} {
+		got := test.v.Rotate(test.alpha, test.q)
+		if !vecApproxEqual(got, test.want) {
+			t.Errorf(
+				"rotate(%v, %v, %v)= %v, want=%v",
+				test.v, test.alpha, test.q, got, test.want,
+			)
+		}
+	}
+}
+
 func vecIsNaN(v Vec) bool {
 	return math.IsNaN(v.X) && math.IsNaN(v.Y)
 }