github.com/gopherd/gonum@v0.0.4/spatial/r3/mat_safe.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  //go:build safe
     6  // +build safe
     7  
     8  // TODO(kortschak): Get rid of this rigmarole if https://golang.org/issue/50118
     9  // is accepted.
    10  
    11  package r3
    12  
    13  import (
    14  	"github.com/gopherd/gonum/blas/blas64"
    15  	"github.com/gopherd/gonum/mat"
    16  )
    17  
    18  type array [9]float64
    19  
    20  // At returns the value of a matrix element at row i, column j.
    21  // At expects indices in the range [0,2].
    22  // It will panic if i or j are out of bounds for the matrix.
    23  func (m *Mat) At(i, j int) float64 {
    24  	if uint(i) > 2 {
    25  		panic(mat.ErrRowAccess)
    26  	}
    27  	if uint(j) > 2 {
    28  		panic(mat.ErrColAccess)
    29  	}
    30  	if m.data == nil {
    31  		m.data = new(array)
    32  	}
    33  	return m.data[i*3+j]
    34  }
    35  
    36  // Set sets the element at row i, column j to the value v.
    37  func (m *Mat) Set(i, j int, v float64) {
    38  	if uint(i) > 2 {
    39  		panic(mat.ErrRowAccess)
    40  	}
    41  	if uint(j) > 2 {
    42  		panic(mat.ErrColAccess)
    43  	}
    44  	if m.data == nil {
    45  		m.data = new(array)
    46  	}
    47  	m.data[i*3+j] = v
    48  }
    49  
    50  // Eye returns the 3×3 Identity matrix
    51  func Eye() *Mat {
    52  	return &Mat{&array{
    53  		1, 0, 0,
    54  		0, 1, 0,
    55  		0, 0, 1,
    56  	}}
    57  }
    58  
    59  // Skew returns the 3×3 skew symmetric matrix (right hand system) of v.
    60  //                  ⎡ 0 -z  y⎤
    61  //  Skew({x,y,z}) = ⎢ z  0 -x⎥
    62  //                  ⎣-y  x  0⎦
    63  func Skew(v Vec) (M *Mat) {
    64  	return &Mat{&array{
    65  		0, -v.Z, v.Y,
    66  		v.Z, 0, -v.X,
    67  		-v.Y, v.X, 0,
    68  	}}
    69  }
    70  
    71  // Mul takes the matrix product of a and b, placing the result in the receiver.
    72  // If the number of columns in a does not equal 3, Mul will panic.
    73  func (m *Mat) Mul(a, b mat.Matrix) {
    74  	ra, ca := a.Dims()
    75  	rb, cb := b.Dims()
    76  	switch {
    77  	case ra != 3:
    78  		panic(mat.ErrShape)
    79  	case cb != 3:
    80  		panic(mat.ErrShape)
    81  	case ca != rb:
    82  		panic(mat.ErrShape)
    83  	}
    84  	if m.data == nil {
    85  		m.data = new(array)
    86  	}
    87  	if ca != 3 {
    88  		// General matrix multiplication for the case where the inner dimension is not 3.
    89  		var t mat.Dense
    90  		t.Mul(a, b)
    91  		copy(m.data[:], t.RawMatrix().Data)
    92  		return
    93  	}
    94  
    95  	a00 := a.At(0, 0)
    96  	b00 := b.At(0, 0)
    97  	a01 := a.At(0, 1)
    98  	b01 := b.At(0, 1)
    99  	a02 := a.At(0, 2)
   100  	b02 := b.At(0, 2)
   101  	a10 := a.At(1, 0)
   102  	b10 := b.At(1, 0)
   103  	a11 := a.At(1, 1)
   104  	b11 := b.At(1, 1)
   105  	a12 := a.At(1, 2)
   106  	b12 := b.At(1, 2)
   107  	a20 := a.At(2, 0)
   108  	b20 := b.At(2, 0)
   109  	a21 := a.At(2, 1)
   110  	b21 := b.At(2, 1)
   111  	a22 := a.At(2, 2)
   112  	b22 := b.At(2, 2)
   113  	*(m.data) = array{
   114  		a00*b00 + a01*b10 + a02*b20, a00*b01 + a01*b11 + a02*b21, a00*b02 + a01*b12 + a02*b22,
   115  		a10*b00 + a11*b10 + a12*b20, a10*b01 + a11*b11 + a12*b21, a10*b02 + a11*b12 + a12*b22,
   116  		a20*b00 + a21*b10 + a22*b20, a20*b01 + a21*b11 + a22*b21, a20*b02 + a21*b12 + a22*b22,
   117  	}
   118  }
   119  
   120  // RawMatrix returns the blas representation of the matrix with the backing
   121  // data of this matrix. Changes to the returned matrix will be reflected in
   122  // the receiver.
   123  func (m *Mat) RawMatrix() blas64.General {
   124  	if m.data == nil {
   125  		m.data = new(array)
   126  	}
   127  	return blas64.General{Rows: 3, Cols: 3, Data: m.data[:], Stride: 3}
   128  }
   129  
   130  // BUG(kortschak): Implementing NewMat without unsafe conversion or slice to
   131  // array pointer conversion leaves it with semantics that do not match the
   132  // sharing semantics that exist in the mat package.
   133  func arrayFrom(vals []float64) *array {
   134  	// TODO(kortschak): Use array conversion when go1.16 is no longer supported.
   135  	var a array
   136  	copy(a[:], vals)
   137  	return &a
   138  }
   139  
   140  // Mat returns a 3×3 rotation matrix corresponding to the receiver. It
   141  // may be used to perform rotations on a 3-vector or to apply the rotation
   142  // to a 3×n matrix of column vectors. If the receiver is not a unit
   143  // quaternion, the returned matrix will not be a pure rotation.
   144  func (r Rotation) Mat() *Mat {
   145  	w, i, j, k := r.Real, r.Imag, r.Jmag, r.Kmag
   146  	ii := 2 * i * i
   147  	jj := 2 * j * j
   148  	kk := 2 * k * k
   149  	wi := 2 * w * i
   150  	wj := 2 * w * j
   151  	wk := 2 * w * k
   152  	ij := 2 * i * j
   153  	jk := 2 * j * k
   154  	ki := 2 * k * i
   155  	return &Mat{&array{
   156  		1 - (jj + kk), ij - wk, ki + wj,
   157  		ij + wk, 1 - (ii + kk), jk - wi,
   158  		ki - wj, jk + wi, 1 - (ii + jj),
   159  	}}
   160  }