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