github.com/jingcheng-WU/gonum@v0.9.1-0.20210323123734-f1a2a11a8f7b/spatial/r3/vector.go (about)

     1  // Copyright ©2019 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/jingcheng-WU/gonum/num/quat"
    11  )
    12  
    13  // Vec is a 3D vector.
    14  type Vec struct {
    15  	X, Y, Z float64
    16  }
    17  
    18  // Add returns the vector sum of p and q.
    19  func Add(p, q Vec) Vec {
    20  	return Vec{
    21  		X: p.X + q.X,
    22  		Y: p.Y + q.Y,
    23  		Z: p.Z + q.Z,
    24  	}
    25  }
    26  
    27  // Sub returns the vector sum of p and -q.
    28  func Sub(p, q Vec) Vec {
    29  	return Vec{
    30  		X: p.X - q.X,
    31  		Y: p.Y - q.Y,
    32  		Z: p.Z - q.Z,
    33  	}
    34  }
    35  
    36  // Scale returns the vector p scaled by f.
    37  func Scale(f float64, p Vec) Vec {
    38  	return Vec{
    39  		X: f * p.X,
    40  		Y: f * p.Y,
    41  		Z: f * p.Z,
    42  	}
    43  }
    44  
    45  // Dot returns the dot product p·q.
    46  func Dot(p, q Vec) float64 {
    47  	return p.X*q.X + p.Y*q.Y + p.Z*q.Z
    48  }
    49  
    50  // Cross returns the cross product p×q.
    51  func Cross(p, q Vec) Vec {
    52  	return Vec{
    53  		p.Y*q.Z - p.Z*q.Y,
    54  		p.Z*q.X - p.X*q.Z,
    55  		p.X*q.Y - p.Y*q.X,
    56  	}
    57  }
    58  
    59  // Rotate returns a new vector, rotated by alpha around the provided axis.
    60  func Rotate(p Vec, alpha float64, axis Vec) Vec {
    61  	return NewRotation(alpha, axis).Rotate(p)
    62  }
    63  
    64  // Norm returns the Euclidean norm of p
    65  //  |p| = sqrt(p_x^2 + p_y^2 + p_z^2).
    66  func Norm(p Vec) float64 {
    67  	return math.Hypot(p.X, math.Hypot(p.Y, p.Z))
    68  }
    69  
    70  // Norm returns the Euclidean squared norm of p
    71  //  |p|^2 = p_x^2 + p_y^2 + p_z^2.
    72  func Norm2(p Vec) float64 {
    73  	return p.X*p.X + p.Y*p.Y + p.Z*p.Z
    74  }
    75  
    76  // Unit returns the unit vector colinear to p.
    77  // Unit returns {NaN,NaN,NaN} for the zero vector.
    78  func Unit(p Vec) Vec {
    79  	if p.X == 0 && p.Y == 0 && p.Z == 0 {
    80  		return Vec{X: math.NaN(), Y: math.NaN(), Z: math.NaN()}
    81  	}
    82  	return Scale(1/Norm(p), p)
    83  }
    84  
    85  // Cos returns the cosine of the opening angle between p and q.
    86  func Cos(p, q Vec) float64 {
    87  	return Dot(p, q) / (Norm(p) * Norm(q))
    88  }
    89  
    90  // Box is a 3D bounding box.
    91  type Box struct {
    92  	Min, Max Vec
    93  }
    94  
    95  // TODO: possibly useful additions to the current rotation API:
    96  //  - create rotations from Euler angles (NewRotationFromEuler?)
    97  //  - create rotations from rotation matrices (NewRotationFromMatrix?)
    98  //  - return the equivalent Euler angles from a Rotation
    99  //  - return the equivalent rotation matrix from a Rotation
   100  //
   101  // Euler angles have issues (see [1] for a discussion).
   102  // We should think carefully before adding them in.
   103  // [1]: http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/
   104  
   105  // Rotation describes a rotation in space.
   106  type Rotation quat.Number
   107  
   108  // NewRotation creates a rotation by alpha, around axis.
   109  func NewRotation(alpha float64, axis Vec) Rotation {
   110  	if alpha == 0 {
   111  		return Rotation{Real: 1}
   112  	}
   113  	q := raise(axis)
   114  	sin, cos := math.Sincos(0.5 * alpha)
   115  	q = quat.Scale(sin/quat.Abs(q), q)
   116  	q.Real += cos
   117  	if len := quat.Abs(q); len != 1 {
   118  		q = quat.Scale(1/len, q)
   119  	}
   120  
   121  	return Rotation(q)
   122  }
   123  
   124  // Rotate returns the rotated vector according to the definition of rot.
   125  func (r Rotation) Rotate(p Vec) Vec {
   126  	if r.isIdentity() {
   127  		return p
   128  	}
   129  	qq := quat.Number(r)
   130  	pp := quat.Mul(quat.Mul(qq, raise(p)), quat.Conj(qq))
   131  	return Vec{X: pp.Imag, Y: pp.Jmag, Z: pp.Kmag}
   132  }
   133  
   134  func (r Rotation) isIdentity() bool {
   135  	return r == Rotation{Real: 1}
   136  }
   137  
   138  func raise(p Vec) quat.Number {
   139  	return quat.Number{Imag: p.X, Jmag: p.Y, Kmag: p.Z}
   140  }