github.com/mojzesh/linmath@v0.0.0-20220615094601-759643be23c3/quat.go (about)

     1  package linmath
     2  
     3  type Quat [4]float32
     4  
     5  func (q *Quat) Identity() {
     6  	q[0] = 0
     7  	q[1] = 0
     8  	q[2] = 0
     9  	q[3] = 1
    10  }
    11  
    12  func (r *Quat) Add(a, b *Quat) {
    13  	for i := 0; i < 4; i++ {
    14  		r[i] = a[i] + b[i]
    15  	}
    16  }
    17  
    18  func (r *Quat) AddVec3(a *Quat, v *Vec3) {
    19  	for i := 0; i < 3; i++ {
    20  		r[i] = a[i] + v[i]
    21  	}
    22  }
    23  
    24  func (r *Quat) Sub(a, b *Quat) {
    25  	for i := 0; i < 4; i++ {
    26  		r[i] = a[i] - b[i]
    27  	}
    28  }
    29  
    30  // MultCross the same as for Vec3
    31  func (r *Quat) MultCross3(a, b *Quat) {
    32  	r[0] = a[1]*b[2] - a[2]*b[1]
    33  	r[1] = a[2]*b[0] - a[0]*b[2]
    34  	r[2] = a[0]*b[1] - a[1]*b[0]
    35  }
    36  
    37  // QuatMultInner the same as for Vec3
    38  func QuatMultInner3(a, b *Quat) (p float32) {
    39  	for i := 0; i < 3; i++ {
    40  		p += b[i] * a[i]
    41  	}
    42  	return p
    43  }
    44  
    45  func (r *Quat) Mult(p, q *Quat) {
    46  	var w = new(Vec3)
    47  	r.MultCross3(p, q)
    48  	w.ScaleQuat(p, q[3])
    49  	r.AddVec3(r, w)
    50  	w.ScaleQuat(q, p[3])
    51  	r.AddVec3(r, w)
    52  
    53  	r[3] = p[3]*q[3] - QuatMultInner3(p, q)
    54  }
    55  
    56  func (r *Quat) Scale(q *Quat, s float32) {
    57  	for i := 0; i < 4; i++ {
    58  		r[i] = q[i] * s
    59  	}
    60  }
    61  
    62  func QuatInnerProduct(a, b *Quat) (p float32) {
    63  	for i := 0; i < 4; i++ {
    64  		p += b[i] * a[i]
    65  	}
    66  	return p
    67  }
    68  
    69  func (r *Quat) Conj(q *Quat) {
    70  	for i := 0; i < 3; i++ {
    71  		r[i] = -q[i]
    72  	}
    73  	r[3] = q[3]
    74  }
    75  
    76  func (q *Quat) Len() float32 {
    77  	return sqrtf(QuatInnerProduct(q, q))
    78  }
    79  
    80  func (r *Quat) Norm(q *Quat) {
    81  	var k float32 = 1.0 / q.Len()
    82  	r.Scale(q, k)
    83  }