spatial/r3: implement Rotate

Updates gonum/gonum#1513.

spatial/r3/vector.go

@@ -4,7 +4,11 @@
 
 package r3
 
-import "math"
+import (
+	"math"
+
+	"gonum.org/v1/gonum/num/quat"
+)
 
 // Vec is a 3D vector.
 type Vec struct {
@@ -49,6 +53,11 @@ func (p Vec) Cross(q Vec) Vec {
 	}
 }
 
+// Rotate returns a new vector, rotated by alpha around the provided axis.
+func (p Vec) Rotate(alpha float64, axis Vec) Vec {
+	return NewRotation(alpha, axis).Rotate(p)
+}
+
 // Norm returns the Euclidean norm of p
 //  |p| = sqrt(p_x^2 + p_y^2 + p_z^2).
 func Norm(p Vec) float64 {
@@ -79,3 +88,50 @@ func Cos(p, q Vec) float64 {
 type Box struct {
 	Min, Max Vec
 }
+
+// TODO: possibly useful additions to the current rotation API:
+//  - create rotations from Euler angles (NewRotationFromEuler?)
+//  - create rotations from rotation matrices (NewRotationFromMatrix?)
+//  - return the equivalent Euler angles from a Rotation
+//  - return the equivalent rotation matrix from a Rotation
+//
+// Euler angles have issues (see [1] for a discussion).
+// We should think carefully before adding them in.
+// [1]: http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/
+
+// Rotation describes a rotation in space.
+type Rotation quat.Number
+
+// NewRotation creates a rotation by alpha, around axis.
+func NewRotation(alpha float64, axis Vec) Rotation {
+	if alpha == 0 {
+		return Rotation{Real: 1}
+	}
+	q := raise(axis)
+	sin, cos := math.Sincos(0.5 * alpha)
+	q = quat.Scale(sin/quat.Abs(q), q)
+	q.Real += cos
+	if len := quat.Abs(q); len != 1 {
+		q = quat.Scale(1/len, q)
+	}
+
+	return Rotation(q)
+}
+
+// Rotate returns the rotated vector according to the definition of rot.
+func (r Rotation) Rotate(p Vec) Vec {
+	if r.isIdentity() {
+		return p
+	}
+	qq := quat.Number(r)
+	pp := quat.Mul(quat.Mul(qq, raise(p)), quat.Conj(qq))
+	return Vec{X: pp.Imag, Y: pp.Jmag, Z: pp.Kmag}
+}
+
+func (r Rotation) isIdentity() bool {
+	return r == Rotation{Real: 1}
+}
+
+func raise(p Vec) quat.Number {
+	return quat.Number{Imag: p.X, Jmag: p.Y, Kmag: p.Z}
+}