diff --git a/spatial/r2/vector.go b/spatial/r2/vector.go index 191a688c..0a19c196 100644 --- a/spatial/r2/vector.go +++ b/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 +} diff --git a/spatial/r2/vector_test.go b/spatial/r2/vector_test.go index 708baa8a..cff02d75 100644 --- a/spatial/r2/vector_test.go +++ b/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) }