github.com/gopherd/gonum@v0.0.4/spatial/r3/rotation.go (about)

     1  // Copyright ©2021 The Gonum Authors. All rights reserved.
     2  // Use of this source code is governed by a BSD-style
     3  // license that can be found in the LICENSE file.
     4  
     5  package r3
     6  
     7  import (
     8  	"math"
     9  
    10  	"github.com/gopherd/gonum/num/quat"
    11  )
    12  
    13  // TODO: possibly useful additions to the current rotation API:
    14  //  - create rotations from Euler angles (NewRotationFromEuler?)
    15  //  - create rotations from rotation matrices (NewRotationFromMatrix?)
    16  //  - return the equivalent Euler angles from a Rotation
    17  //
    18  // Euler angles have issues (see [1] for a discussion).
    19  // We should think carefully before adding them in.
    20  // [1]: http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/
    21  
    22  // Rotation describes a rotation in space.
    23  type Rotation quat.Number
    24  
    25  // NewRotation creates a rotation by alpha, around axis.
    26  func NewRotation(alpha float64, axis Vec) Rotation {
    27  	if alpha == 0 {
    28  		return Rotation{Real: 1}
    29  	}
    30  	q := raise(axis)
    31  	sin, cos := math.Sincos(0.5 * alpha)
    32  	q = quat.Scale(sin/quat.Abs(q), q)
    33  	q.Real += cos
    34  	if len := quat.Abs(q); len != 1 {
    35  		q = quat.Scale(1/len, q)
    36  	}
    37  
    38  	return Rotation(q)
    39  }
    40  
    41  // Rotate returns p rotated according to the parameters used to construct
    42  // the receiver.
    43  func (r Rotation) Rotate(p Vec) Vec {
    44  	if r.isIdentity() {
    45  		return p
    46  	}
    47  	qq := quat.Number(r)
    48  	pp := quat.Mul(quat.Mul(qq, raise(p)), quat.Conj(qq))
    49  	return Vec{X: pp.Imag, Y: pp.Jmag, Z: pp.Kmag}
    50  }
    51  
    52  func (r Rotation) isIdentity() bool {
    53  	return r == Rotation{Real: 1}
    54  }
    55  
    56  func raise(p Vec) quat.Number {
    57  	return quat.Number{Imag: p.X, Jmag: p.Y, Kmag: p.Z}
    58  }