mathf/quaternion.go at main · godot-ext/mathf · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
package mathf

import (
	"fmt"
	"math"
	"unsafe"

	"github.com/realdream-ai/mathf/impl"
)

type Quaternion struct {
	X, Y, Z, W Float
}

func (q Quaternion) toImpl() impl.Quaternion {
	return *(*impl.Quaternion)(unsafe.Pointer(&q))
}

func (q Quaternion) fromImpl(iq impl.Quaternion) Quaternion {
	return *(*Quaternion)(unsafe.Pointer(&iq))
}

func NewQuaternion(x, y, z, w float64) Quaternion {
	return Quaternion{Float(x), Float(y), Float(z), Float(w)}
}

func NewQuaternionFromAxisAngle(axis Vec3, angle float64) Quaternion {
	halfAngle := angle * 0.5
	s := math.Sin(halfAngle)
	return Quaternion{
		X: Float(axis.X * Float(s)),
		Y: Float(axis.Y * Float(s)),
		Z: Float(axis.Z * Float(s)),
		W: Float(math.Cos(halfAngle)),
	}
}

func (q Quaternion) Add(other Quaternion) Quaternion {
	return q.fromImpl(q.toImpl().Add(other.toImpl()))
}

func (q Quaternion) Sub(other Quaternion) Quaternion {
	return q.fromImpl(q.toImpl().Sub(other.toImpl()))
}

func (q Quaternion) Mul(other Quaternion) Quaternion {
	return q.fromImpl(q.toImpl().Mul(other.toImpl()))
}

func (q Quaternion) Mulf(f float64) Quaternion {
	return q.fromImpl(q.toImpl().Mulf(f))
}

func (q Quaternion) Dot(other Quaternion) float64 {
	return q.toImpl().Dot(other.toImpl())
}

func (q Quaternion) Length() float64 {
	return q.toImpl().Length()
}

func (q Quaternion) LengthSquared() float64 {
	return q.toImpl().LengthSquared()
}

func (q Quaternion) Normalize() Quaternion {
	return q.fromImpl(q.toImpl().Normalized())
}

func (q Quaternion) IsNormalized() bool {
	return q.toImpl().IsNormalized()
}

func (q Quaternion) IsFinite() bool {
	return q.toImpl().IsFinite()
}

func (q Quaternion) Inverse() Quaternion {
	return q.fromImpl(q.toImpl().Inverse())
}

func (q Quaternion) Conjugate() Quaternion {
	return q.fromImpl(q.toImpl().Inverse()) // Conjugate is the same as inverse for unit quaternions
}

func (q Quaternion) Lerp(to Quaternion, weight float64) Quaternion {
	return q.fromImpl(q.toImpl().Slerpni(to.toImpl(), weight))
}

func (q Quaternion) Lerpf(to Quaternion, weight float64) Quaternion {
	return q.fromImpl(q.toImpl().Slerpni(to.toImpl(), weight))
}

func (q Quaternion) Slerp(to Quaternion, weight float64) Quaternion {
	return q.fromImpl(q.toImpl().Slerp(to.toImpl(), weight))
}

func (q Quaternion) Slerpf(to Quaternion, weight float64) Quaternion {
	return q.fromImpl(q.toImpl().Slerp(to.toImpl(), weight))
}

func (q Quaternion) GetEuler() Vec3 {
	angles := q.toImpl().EulerAngles(2) // EulerOrderYXZ = 2
	return NewVec3(float64(angles[0]), float64(angles[1]), float64(angles[2]))
}

func (q Quaternion) RotateVec3(v Vec3) Vec3 {
	// Convert quaternion to basis and use it to rotate the Vec
	qv := NewQuaternion(float64(v.X), float64(v.Y), float64(v.Z), 0)
	qr := q.Mul(qv).Mul(q.Conjugate())
	return NewVec3(float64(qr.X), float64(qr.Y), float64(qr.Z))
}

func (q Quaternion) Neg() Quaternion {
	return q.fromImpl(q.toImpl().Neg())
}

func (q Quaternion) String() string {
	return fmt.Sprintf("[x=%g, y=%g, z=%g, w=%g]", q.X, q.Y, q.Z, q.W)
}