diff --git a/spatial/r3/vector.go b/spatial/r3/vector.go index 3ec64e40..46d64922 100644 --- a/spatial/r3/vector.go +++ b/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), + }) +} diff --git a/spatial/r3/vector_test.go b/spatial/r3/vector_test.go index d9a85988..e24a2171 100644 --- a/spatial/r3/vector_test.go +++ b/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) }