matrix.go

package sdf

import (
	"math"
	"math/rand"

	"github.com/soypat/sdf/internal/d2"
	"github.com/soypat/sdf/internal/d3"
	"gonum.org/v1/gonum/spatial/r2"
	"gonum.org/v1/gonum/spatial/r3"
)

// m44 is a 4x4 matrix.
type m44 struct {
	x00, x01, x02, x03 float64
	x10, x11, x12, x13 float64
	x20, x21, x22, x23 float64
	x30, x31, x32, x33 float64
}

// m33 is a 3x3 matrix.
type m33 struct {
	x00, x01, x02 float64
	x10, x11, x12 float64
	x20, x21, x22 float64
}

// m22 is a 2x2 matrix.
type m22 struct {
	x00, x01 float64
	x10, x11 float64
}

// randomRange returns a random float64 [a,b)
func randomRange(a, b float64) float64 {
	return a + (b-a)*rand.Float64()
}

// RandomM22 returns a 2x2 matrix with random elements.
// func RandomM22(a, b float64) M22 {
// 	m := M22{randomRange(a, b),
// 		randomRange(a, b),
// 		randomRange(a, b),
// 		randomRange(a, b)}
// 	return m
// }

// RandomM33 returns a 3x3 matrix with random elements.
// func RandomM33(a, b float64) M33 {
// 	m := M33{randomRange(a, b),
// 		randomRange(a, b),
// 		randomRange(a, b),
// 		randomRange(a, b),
// 		randomRange(a, b),
// 		randomRange(a, b),
// 		randomRange(a, b),
// 		randomRange(a, b),
// 		randomRange(a, b)}
// 	return m
// }

// randomM44 returns a 4x4 matrix with random elements.
func randomM44(a, b float64) m44 {
	m := m44{
		randomRange(a, b),
		randomRange(a, b),
		randomRange(a, b),
		randomRange(a, b),
		randomRange(a, b),
		randomRange(a, b),
		randomRange(a, b),
		randomRange(a, b),
		randomRange(a, b),
		randomRange(a, b),
		randomRange(a, b),
		randomRange(a, b),
		randomRange(a, b),
		randomRange(a, b),
		randomRange(a, b),
		randomRange(a, b)}
	return m
}

// identity3d returns a 4x4 identity matrix.
func identity3d() m44 {
	return m44{
		1, 0, 0, 0,
		0, 1, 0, 0,
		0, 0, 1, 0,
		0, 0, 0, 1}
}

// identity2d returns a 3x3 identity matrix.
func identity2d() m33 {
	return m33{
		1, 0, 0,
		0, 1, 0,
		0, 0, 1}
}

// identity returns a 2x2 identity matrix.
func identity() m22 {
	return m22{
		1, 0,
		0, 1}
}

// Translate3D returns a 4x4 translation matrix.
func Translate3D(v r3.Vec) m44 {
	return m44{
		1, 0, 0, v.X,
		0, 1, 0, v.Y,
		0, 0, 1, v.Z,
		0, 0, 0, 1}
}

// Translate2D returns a 3x3 translation matrix.
func Translate2D(v r2.Vec) m33 {
	return m33{
		1, 0, v.X,
		0, 1, v.Y,
		0, 0, 1}
}

// Scale3D returns a 4x4 scaling matrix.
// Scaling does not preserve distance. See: ScaleUniform3D()
func Scale3D(v r3.Vec) m44 {
	return m44{
		v.X, 0, 0, 0,
		0, v.Y, 0, 0,
		0, 0, v.Z, 0,
		0, 0, 0, 1}
}

// Scale2D returns a 3x3 scaling matrix.
// Scaling does not preserve distance. See: ScaleUniform2D().
func Scale2D(v r2.Vec) m33 {
	return m33{
		v.X, 0, 0,
		0, v.Y, 0,
		0, 0, 1}
}

// Rotate3D returns an orthographic 4x4 rotation matrix (right hand rule).
func Rotate3D(v r3.Vec, a float64) m44 {
	v = r3.Unit(v)
	s, c := math.Sincos(a)
	m := 1 - c
	return m44{
		m*v.X*v.X + c, m*v.X*v.Y - v.Z*s, m*v.Z*v.X + v.Y*s, 0,
		m*v.X*v.Y + v.Z*s, m*v.Y*v.Y + c, m*v.Y*v.Z - v.X*s, 0,
		m*v.Z*v.X - v.Y*s, m*v.Y*v.Z + v.X*s, m*v.Z*v.Z + c, 0,
		0, 0, 0, 1,
	}
}

// RotateX returns a 4x4 matrix with rotation about the X axis.
func RotateX(a float64) m44 {
	return Rotate3D(r3.Vec{X: 1, Y: 0, Z: 0}, a)
}

// RotateY returns a 4x4 matrix with rotation about the Y axis.
func RotateY(a float64) m44 {
	return Rotate3D(r3.Vec{X: 0, Y: 1, Z: 0}, a)
}

// RotateZ returns a 4x4 matrix with rotation about the Z axis.
func RotateZ(a float64) m44 {
	return Rotate3D(r3.Vec{X: 0, Y: 0, Z: 1}, a)
}

// MirrorXY returns a 4x4 matrix with mirroring across the XY plane.
func MirrorXY() m44 {
	return m44{
		1, 0, 0, 0,
		0, 1, 0, 0,
		0, 0, -1, 0,
		0, 0, 0, 1}
}

// MirrorXZ returns a 4x4 matrix with mirroring across the XZ plane.
func MirrorXZ() m44 {
	return m44{
		1, 0, 0, 0,
		0, -1, 0, 0,
		0, 0, 1, 0,
		0, 0, 0, 1}
}

// MirrorYZ returns a 4x4 matrix with mirroring across the YZ plane.
func MirrorYZ() m44 {
	return m44{
		-1, 0, 0, 0,
		0, 1, 0, 0,
		0, 0, 1, 0,
		0, 0, 0, 1}
}

// MirrorXeqY returns a 4x4 matrix with mirroring across the X == Y plane.
func MirrorXeqY() m44 {
	return m44{
		0, 1, 0, 0,
		1, 0, 0, 0,
		0, 0, 1, 0,
		0, 0, 0, 1}
}

// MirrorX returns a 3x3 matrix with mirroring across the X axis.
func MirrorX() m33 {
	return m33{
		1, 0, 0,
		0, -1, 0,
		0, 0, 1}
}

// MirrorY returns a 3x3 matrix with mirroring across the Y axis.
func MirrorY() m33 {
	return m33{
		-1, 0, 0,
		0, 1, 0,
		0, 0, 1}
}

// Rotate2D returns an orthographic 3x3 rotation matrix (right hand rule).
func Rotate2D(a float64) m33 {
	s := math.Sin(a)
	c := math.Cos(a)
	return m33{
		c, -s, 0,
		s, c, 0,
		0, 0, 1}
}

// Rotate returns an orthographic 2x2 rotation matrix (right hand rule).
func Rotate(a float64) m22 {
	s := math.Sin(a)
	c := math.Cos(a)
	return m22{
		c, -s,
		s, c,
	}
}

// equals tests the equality of 4x4 matrices.
func (a m44) equals(b m44, tolerance float64) bool {
	return (math.Abs(a.x00-b.x00) < tolerance &&
		math.Abs(a.x01-b.x01) < tolerance &&
		math.Abs(a.x02-b.x02) < tolerance &&
		math.Abs(a.x03-b.x03) < tolerance &&
		math.Abs(a.x10-b.x10) < tolerance &&
		math.Abs(a.x11-b.x11) < tolerance &&
		math.Abs(a.x12-b.x12) < tolerance &&
		math.Abs(a.x13-b.x13) < tolerance &&
		math.Abs(a.x20-b.x20) < tolerance &&
		math.Abs(a.x21-b.x21) < tolerance &&
		math.Abs(a.x22-b.x22) < tolerance &&
		math.Abs(a.x23-b.x23) < tolerance &&
		math.Abs(a.x30-b.x30) < tolerance &&
		math.Abs(a.x31-b.x31) < tolerance &&
		math.Abs(a.x32-b.x32) < tolerance &&
		math.Abs(a.x33-b.x33) < tolerance)
}

// equals tests the equality of 3x3 matrices.
func (a m33) equals(b m33, tolerance float64) bool {
	return (math.Abs(a.x00-b.x00) < tolerance &&
		math.Abs(a.x01-b.x01) < tolerance &&
		math.Abs(a.x02-b.x02) < tolerance &&
		math.Abs(a.x10-b.x10) < tolerance &&
		math.Abs(a.x11-b.x11) < tolerance &&
		math.Abs(a.x12-b.x12) < tolerance &&
		math.Abs(a.x20-b.x20) < tolerance &&
		math.Abs(a.x21-b.x21) < tolerance &&
		math.Abs(a.x22-b.x22) < tolerance)
}

// equals tests the equality of 2x2 matrices.
func (a m22) equals(b m22, tolerance float64) bool {
	return (math.Abs(a.x00-b.x00) < tolerance &&
		math.Abs(a.x01-b.x01) < tolerance &&
		math.Abs(a.x10-b.x10) < tolerance &&
		math.Abs(a.x11-b.x11) < tolerance)
}

// MulPosition multiplies a V2 position with a rotate/translate matrix.
func (a m33) MulPosition(b r2.Vec) r2.Vec {
	return r2.Vec{X: a.x00*b.X + a.x01*b.Y + a.x02,
		Y: a.x10*b.X + a.x11*b.Y + a.x12}
}

// MulPosition multiplies a V2 position with a rotate matrix.
func (a m22) MulPosition(b r2.Vec) r2.Vec {
	return r2.Vec{X: a.x00*b.X + a.x01*b.Y,
		Y: a.x10*b.X + a.x11*b.Y}
}

// Mul multiplies 4x4 matrices.
func (a m44) Mul(b m44) m44 {
	m := m44{}
	m.x00 = a.x00*b.x00 + a.x01*b.x10 + a.x02*b.x20 + a.x03*b.x30
	m.x10 = a.x10*b.x00 + a.x11*b.x10 + a.x12*b.x20 + a.x13*b.x30
	m.x20 = a.x20*b.x00 + a.x21*b.x10 + a.x22*b.x20 + a.x23*b.x30
	m.x30 = a.x30*b.x00 + a.x31*b.x10 + a.x32*b.x20 + a.x33*b.x30
	m.x01 = a.x00*b.x01 + a.x01*b.x11 + a.x02*b.x21 + a.x03*b.x31
	m.x11 = a.x10*b.x01 + a.x11*b.x11 + a.x12*b.x21 + a.x13*b.x31
	m.x21 = a.x20*b.x01 + a.x21*b.x11 + a.x22*b.x21 + a.x23*b.x31
	m.x31 = a.x30*b.x01 + a.x31*b.x11 + a.x32*b.x21 + a.x33*b.x31
	m.x02 = a.x00*b.x02 + a.x01*b.x12 + a.x02*b.x22 + a.x03*b.x32
	m.x12 = a.x10*b.x02 + a.x11*b.x12 + a.x12*b.x22 + a.x13*b.x32
	m.x22 = a.x20*b.x02 + a.x21*b.x12 + a.x22*b.x22 + a.x23*b.x32
	m.x32 = a.x30*b.x02 + a.x31*b.x12 + a.x32*b.x22 + a.x33*b.x32
	m.x03 = a.x00*b.x03 + a.x01*b.x13 + a.x02*b.x23 + a.x03*b.x33
	m.x13 = a.x10*b.x03 + a.x11*b.x13 + a.x12*b.x23 + a.x13*b.x33
	m.x23 = a.x20*b.x03 + a.x21*b.x13 + a.x22*b.x23 + a.x23*b.x33
	m.x33 = a.x30*b.x03 + a.x31*b.x13 + a.x32*b.x23 + a.x33*b.x33
	return m
}

// Mul multiplies 3x3 matrices.
func (a m33) Mul(b m33) m33 {
	m := m33{}
	m.x00 = a.x00*b.x00 + a.x01*b.x10 + a.x02*b.x20
	m.x10 = a.x10*b.x00 + a.x11*b.x10 + a.x12*b.x20
	m.x20 = a.x20*b.x00 + a.x21*b.x10 + a.x22*b.x20
	m.x01 = a.x00*b.x01 + a.x01*b.x11 + a.x02*b.x21
	m.x11 = a.x10*b.x01 + a.x11*b.x11 + a.x12*b.x21
	m.x21 = a.x20*b.x01 + a.x21*b.x11 + a.x22*b.x21
	m.x02 = a.x00*b.x02 + a.x01*b.x12 + a.x02*b.x22
	m.x12 = a.x10*b.x02 + a.x11*b.x12 + a.x12*b.x22
	m.x22 = a.x20*b.x02 + a.x21*b.x12 + a.x22*b.x22
	return m
}

// Mul multiplies 2x2 matrices.
func (a m22) Mul(b m22) m22 {
	m := m22{}
	m.x00 = a.x00*b.x00 + a.x01*b.x10
	m.x01 = a.x00*b.x01 + a.x01*b.x11
	m.x10 = a.x10*b.x00 + a.x11*b.x10
	m.x11 = a.x10*b.x01 + a.x11*b.x11
	return m
}

// Add two 3x3 matrices.
func (a m33) Add(b m33) m33 {
	return m33{
		x00: a.x00 + b.x00,
		x10: a.x10 + b.x10,
		x20: a.x20 + b.x20,
		x01: a.x01 + b.x01,
		x11: a.x11 + b.x11,
		x21: a.x21 + b.x21,
		x02: a.x02 + b.x02,
		x12: a.x12 + b.x12,
		x22: a.x22 + b.x22,
	}
}

// MulScalar multiplies each 3x3 matrix component by a scalar.
func (a m33) MulScalar(k float64) m33 {
	return m33{
		x00: k * a.x00,
		x10: k * a.x10,
		x20: k * a.x20,
		x01: k * a.x01,
		x11: k * a.x11,
		x21: k * a.x21,
		x02: k * a.x02,
		x12: k * a.x12,
		x22: k * a.x22,
	}
}

// Transform bounding boxes - keep them axis aligned
// http://dev.theomader.com/transform-bounding-boxes/

// MulPosition multiplies a r3.Vec position with a rotate/translate matrix.
func (a m44) MulPosition(b r3.Vec) r3.Vec {
	return r3.Vec{
		X: a.x00*b.X + a.x01*b.Y + a.x02*b.Z + a.x03,
		Y: a.x10*b.X + a.x11*b.Y + a.x12*b.Z + a.x13,
		Z: a.x20*b.X + a.x21*b.Y + a.x22*b.Z + a.x23}
}

// MulBox rotates/translates a 3d bounding box and resizes for axis-alignment.
func (a m44) MulBox(box r3.Box) r3.Box {
	r := r3.Vec{X: a.x00, Y: a.x10, Z: a.x20}
	u := r3.Vec{X: a.x01, Y: a.x11, Z: a.x21}
	b := r3.Vec{X: a.x02, Y: a.x12, Z: a.x22}
	t := r3.Vec{X: a.x03, Y: a.x13, Z: a.x23}

	xa := r3.Scale(box.Min.X, r)
	xb := r3.Scale(box.Max.X, r)
	ya := r3.Scale(box.Min.Y, u)
	yb := r3.Scale(box.Max.Y, u)
	za := r3.Scale(box.Min.Z, b)
	zb := r3.Scale(box.Max.Z, b)
	xa, xb = d3.MinElem(xa, xb), d3.MaxElem(xa, xb)
	ya, yb = d3.MinElem(ya, yb), d3.MaxElem(ya, yb)
	za, zb = d3.MinElem(za, zb), d3.MaxElem(za, zb)
	min := r3.Add(xa, r3.Add(ya, r3.Add(za, t)))
	max := r3.Add(xb, r3.Add(yb, r3.Add(zb, t)))
	return r3.Box{Min: min, Max: max}
}

// MulBox rotates/translates a 2d bounding box and resizes for axis-alignment.
func (a m33) MulBox(box r2.Box) r2.Box {
	r := r2.Vec{X: a.x00, Y: a.x10}
	u := r2.Vec{X: a.x01, Y: a.x11}
	t := r2.Vec{X: a.x02, Y: a.x12}
	xa := r2.Scale(box.Min.X, r)
	xb := r2.Scale(box.Max.X, r)
	ya := r2.Scale(box.Min.Y, u)
	yb := r2.Scale(box.Max.Y, u)
	xa, xb = d2.MinElem(xa, xb), d2.MaxElem(xa, xb)
	ya, yb = d2.MinElem(ya, yb), d2.MaxElem(ya, yb)
	min := r2.Add(xa, r2.Add(ya, t))
	max := r2.Add(xb, r2.Add(yb, t))
	// min := xa.Add(ya).Add(t)
	// max := xb.Add(yb).Add(t)
	return r2.Box{Min: min, Max: max}
}

// Determinant returns the determinant of a 4x4 matrix.
func (a m44) Determinant() float64 {
	return (a.x00*a.x11*a.x22*a.x33 - a.x00*a.x11*a.x23*a.x32 +
		a.x00*a.x12*a.x23*a.x31 - a.x00*a.x12*a.x21*a.x33 +
		a.x00*a.x13*a.x21*a.x32 - a.x00*a.x13*a.x22*a.x31 -
		a.x01*a.x12*a.x23*a.x30 + a.x01*a.x12*a.x20*a.x33 -
		a.x01*a.x13*a.x20*a.x32 + a.x01*a.x13*a.x22*a.x30 -
		a.x01*a.x10*a.x22*a.x33 + a.x01*a.x10*a.x23*a.x32 +
		a.x02*a.x13*a.x20*a.x31 - a.x02*a.x13*a.x21*a.x30 +
		a.x02*a.x10*a.x21*a.x33 - a.x02*a.x10*a.x23*a.x31 +
		a.x02*a.x11*a.x23*a.x30 - a.x02*a.x11*a.x20*a.x33 -
		a.x03*a.x10*a.x21*a.x32 + a.x03*a.x10*a.x22*a.x31 -
		a.x03*a.x11*a.x22*a.x30 + a.x03*a.x11*a.x20*a.x32 -
		a.x03*a.x12*a.x20*a.x31 + a.x03*a.x12*a.x21*a.x30)
}

// Determinant returns the determinant of a 3x3 matrix.
func (a m33) Determinant() float64 {
	return (a.x00*(a.x11*a.x22-a.x21*a.x12) -
		a.x01*(a.x10*a.x22-a.x20*a.x12) +
		a.x02*(a.x10*a.x21-a.x20*a.x11))
}

// Determinant returns the determinant of a 2x2 matrix.
func (a m22) Determinant() float64 {
	return a.x00*a.x11 - a.x01*a.x10
}

// Inverse returns the inverse of a 4x4 matrix.
func (a m44) Inverse() m44 {
	m := m44{}
	d := 1 / a.Determinant()
	m.x00 = (a.x12*a.x23*a.x31 - a.x13*a.x22*a.x31 + a.x13*a.x21*a.x32 - a.x11*a.x23*a.x32 - a.x12*a.x21*a.x33 + a.x11*a.x22*a.x33) * d
	m.x01 = (a.x03*a.x22*a.x31 - a.x02*a.x23*a.x31 - a.x03*a.x21*a.x32 + a.x01*a.x23*a.x32 + a.x02*a.x21*a.x33 - a.x01*a.x22*a.x33) * d
	m.x02 = (a.x02*a.x13*a.x31 - a.x03*a.x12*a.x31 + a.x03*a.x11*a.x32 - a.x01*a.x13*a.x32 - a.x02*a.x11*a.x33 + a.x01*a.x12*a.x33) * d
	m.x03 = (a.x03*a.x12*a.x21 - a.x02*a.x13*a.x21 - a.x03*a.x11*a.x22 + a.x01*a.x13*a.x22 + a.x02*a.x11*a.x23 - a.x01*a.x12*a.x23) * d
	m.x10 = (a.x13*a.x22*a.x30 - a.x12*a.x23*a.x30 - a.x13*a.x20*a.x32 + a.x10*a.x23*a.x32 + a.x12*a.x20*a.x33 - a.x10*a.x22*a.x33) * d
	m.x11 = (a.x02*a.x23*a.x30 - a.x03*a.x22*a.x30 + a.x03*a.x20*a.x32 - a.x00*a.x23*a.x32 - a.x02*a.x20*a.x33 + a.x00*a.x22*a.x33) * d
	m.x12 = (a.x03*a.x12*a.x30 - a.x02*a.x13*a.x30 - a.x03*a.x10*a.x32 + a.x00*a.x13*a.x32 + a.x02*a.x10*a.x33 - a.x00*a.x12*a.x33) * d
	m.x13 = (a.x02*a.x13*a.x20 - a.x03*a.x12*a.x20 + a.x03*a.x10*a.x22 - a.x00*a.x13*a.x22 - a.x02*a.x10*a.x23 + a.x00*a.x12*a.x23) * d
	m.x20 = (a.x11*a.x23*a.x30 - a.x13*a.x21*a.x30 + a.x13*a.x20*a.x31 - a.x10*a.x23*a.x31 - a.x11*a.x20*a.x33 + a.x10*a.x21*a.x33) * d
	m.x21 = (a.x03*a.x21*a.x30 - a.x01*a.x23*a.x30 - a.x03*a.x20*a.x31 + a.x00*a.x23*a.x31 + a.x01*a.x20*a.x33 - a.x00*a.x21*a.x33) * d
	m.x22 = (a.x01*a.x13*a.x30 - a.x03*a.x11*a.x30 + a.x03*a.x10*a.x31 - a.x00*a.x13*a.x31 - a.x01*a.x10*a.x33 + a.x00*a.x11*a.x33) * d
	m.x23 = (a.x03*a.x11*a.x20 - a.x01*a.x13*a.x20 - a.x03*a.x10*a.x21 + a.x00*a.x13*a.x21 + a.x01*a.x10*a.x23 - a.x00*a.x11*a.x23) * d
	m.x30 = (a.x12*a.x21*a.x30 - a.x11*a.x22*a.x30 - a.x12*a.x20*a.x31 + a.x10*a.x22*a.x31 + a.x11*a.x20*a.x32 - a.x10*a.x21*a.x32) * d
	m.x31 = (a.x01*a.x22*a.x30 - a.x02*a.x21*a.x30 + a.x02*a.x20*a.x31 - a.x00*a.x22*a.x31 - a.x01*a.x20*a.x32 + a.x00*a.x21*a.x32) * d
	m.x32 = (a.x02*a.x11*a.x30 - a.x01*a.x12*a.x30 - a.x02*a.x10*a.x31 + a.x00*a.x12*a.x31 + a.x01*a.x10*a.x32 - a.x00*a.x11*a.x32) * d
	m.x33 = (a.x01*a.x12*a.x20 - a.x02*a.x11*a.x20 + a.x02*a.x10*a.x21 - a.x00*a.x12*a.x21 - a.x01*a.x10*a.x22 + a.x00*a.x11*a.x22) * d
	return m
}

// Inverse returns the inverse of a 3x3 matrix.
func (a m33) Inverse() m33 {
	m := m33{}
	d := 1 / a.Determinant()
	m.x00 = (a.x11*a.x22 - a.x12*a.x21) * d
	m.x01 = (a.x21*a.x02 - a.x01*a.x22) * d
	m.x02 = (a.x01*a.x12 - a.x11*a.x02) * d
	m.x10 = (a.x12*a.x20 - a.x22*a.x10) * d
	m.x11 = (a.x22*a.x00 - a.x20*a.x02) * d
	m.x12 = (a.x02*a.x10 - a.x12*a.x00) * d
	m.x20 = (a.x10*a.x21 - a.x20*a.x11) * d
	m.x21 = (a.x20*a.x01 - a.x00*a.x21) * d
	m.x22 = (a.x00*a.x11 - a.x01*a.x10) * d
	return m
}

// Inverse returns the inverse of a 2x2 matrix.
func (a m22) Inverse() m22 {
	m := m22{}
	d := 1 / a.Determinant()
	m.x00 = a.x11 * d
	m.x01 = -a.x01 * d
	m.x10 = -a.x10 * d
	m.x11 = a.x00 * d
	return m
}

// rotateToVector returns the rotation matrix that transforms a onto the same direction as b.
func rotateToVec(a, b r3.Vec) m44 {
	// is either vector == 0?
	if d3.EqualWithin(a, r3.Vec{}, epsilon) || d3.EqualWithin(b, r3.Vec{}, epsilon) {
		return identity3d()
	}
	// normalize both vectors
	a = r3.Unit(a)
	b = r3.Unit(b)
	// are the vectors the same?
	if d3.EqualWithin(a, b, epsilon) {
		return identity3d()
	}

	// are the vectors opposite (180 degrees apart)?
	if d3.EqualWithin(r3.Scale(-1, a), b, epsilon) {
		return m44{
			-1, 0, 0, 0,
			0, -1, 0, 0,
			0, 0, -1, 0,
			0, 0, 0, 1,
		}
	}
	// general case
	// See:	https://math.stackexchange.com/questions/180418/calculate-rotation-matrix-to-align-vector-a-to-vector-b-in-3d
	v := r3.Cross(a, b)
	vx := r3.Skew(v)

	k := 1 / (1 + r3.Dot(a, b))
	vx2 := r3.NewMat(nil)
	vx2.Mul(vx, vx)
	vx2.Scale(k, vx2)

	// Calculate sum of matrices.
	vx.Add(vx, r3.Eye())
	vx.Add(vx, vx2)
	return m44{
		vx.At(0, 0), vx.At(0, 1), vx.At(0, 2), 0,
		vx.At(1, 0), vx.At(1, 1), vx.At(1, 2), 0,
		vx.At(2, 0), vx.At(2, 1), vx.At(2, 2), 0,
		0, 0, 0, 1,
	}
}