spatial/r3: add method to obtain a rotation matrix

spatial/r3/vector.go

@@ -7,6 +7,7 @@ package r3
 import (
 	"math"
 
+	"gonum.org/v1/gonum/mat"
 	"gonum.org/v1/gonum/num/quat"
 )
 
@@ -96,7 +97,6 @@ type Box struct {
 //  - 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.
@@ -139,3 +139,25 @@ func (r Rotation) isIdentity() bool {
 func raise(p Vec) quat.Number {
 	return quat.Number{Imag: p.X, Jmag: p.Y, Kmag: p.Z}
 }
+
+// Matrix returns a 3×3 rotation matrix corresponding to the receiver. It
+// may be used to perform rotations on a 3-vector or to apply the rotation
+// to a 3×n matrix of column vectors. If the receiver is not a unit
+// quaternion, the returned matrix will not be a pure rotation.
+func (r Rotation) Matrix() mat.Matrix {
+	re, im, jm, km := r.Real, r.Imag, r.Jmag, r.Kmag
+	im2 := im * im
+	jm2 := jm * jm
+	km2 := km * km
+	rim := re * im
+	rjm := re * jm
+	rkm := re * km
+	ijm := im * jm
+	jkm := jm * km
+	kim := km * im
+	return mat.NewDense(3, 3, []float64{
+		1 - 2*(jm2+km2), 2 * (ijm - rkm), 2 * (kim + rjm),
+		2 * (ijm + rkm), 1 - 2*(im2+km2), 2 * (jkm - rim),
+		2 * (kim - rjm), 2 * (jkm + rim), 1 - 2*(im2+jm2),
+	})
+}

spatial/r3/vector_test.go

@@ -9,6 +9,7 @@ import (
 	"testing"
 
 	"gonum.org/v1/gonum/floats/scalar"
+	"gonum.org/v1/gonum/mat"
 )
 
 func TestAdd(t *testing.T) {
@@ -236,13 +237,34 @@ func TestRotate(t *testing.T) {
 		got := Rotate(test.v, test.alpha, test.axis)
 		if !vecApproxEqual(got, test.want, tol) {
 			t.Errorf(
-				"rotate(%v, %v, %v)= %v, want=%v",
+				"quat rotate(%v, %v, %v)= %v, want=%v",
+				test.v, test.alpha, test.axis, got, test.want,
+			)
+		}
+
+		var gotv mat.VecDense
+		gotv.MulVec(NewRotation(test.alpha, test.axis).Matrix(), vecDense(test.v))
+		got = vec(gotv)
+		if !vecApproxEqual(got, test.want, tol) {
+			t.Errorf(
+				"matrix rotate(%v, %v, %v)= %v, want=%v",
 				test.v, test.alpha, test.axis, got, test.want,
 			)
 		}
 	}
 }
 
+func vecDense(v Vec) *mat.VecDense {
+	return mat.NewVecDense(3, []float64{v.X, v.Y, v.Z})
+}
+
+func vec(v mat.VecDense) Vec {
+	if v.Len() != 3 {
+		panic(mat.ErrShape)
+	}
+	return Vec{v.AtVec(0), v.AtVec(1), v.AtVec(2)}
+}
+
 func vecIsNaN(v Vec) bool {
 	return math.IsNaN(v.X) && math.IsNaN(v.Y) && math.IsNaN(v.Z)
 }