spatial/r2: implement Rotate
Fixes gonum/gonum#1513.
diff --git a/spatial/r2/vector.go b/spatial/r2/vector.go
index 191a688..0a19c19 100644
--- a/spatial/r2/vector.go
+++ b/spatial/r2/vector.go
@@ -42,6 +42,11 @@
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 @@
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
+}
diff --git a/spatial/r2/vector_test.go b/spatial/r2/vector_test.go
index 708baa8..cff02d7 100644
--- a/spatial/r2/vector_test.go
+++ b/spatial/r2/vector_test.go
@@ -240,6 +240,31 @@
}
}
+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)
}
