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
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
|
#include "mons_math/quat.h"
#include "mons_math/mat4.h"
#include "mons_math/vec3.h"
#include "mons_math/vec4.h"
#include <math.h>
#include <stdlib.h>
#include <stdio.h>
mons_quat mons_quat_from_axis_angle(mons_vec3 axis, float angle) {
float half_angle = 0.5f * angle;
float s = sinf(half_angle);
return (mons_quat){
axis.x * s,
axis.y * s,
axis.z * s,
cosf(half_angle),
};
}
mons_quat mons_quat_mul(mons_quat a, mons_quat b) {
return (mons_quat){
(a.w * b.x) + (a.x * b.w) + (a.y * b.z) - (a.z * b.y),
(a.w * b.y) + (a.y * b.w) + (a.z * b.x) - (a.x * b.z),
(a.w * b.z) + (a.z * b.w) + (a.x * b.y) - (a.y * b.x),
(a.w * b.w) - (a.x * b.x) - (a.y * b.y) - (a.z * b.z),
};
}
void mons_quat_mul_inplace(mons_quat *a, mons_quat b) {
*a = mons_quat_mul(*a, b);
}
mons_quat mons_quat_mul_f(mons_quat a, float b) {
return (mons_quat){
a.x * b,
a.y * b,
a.z * b,
a.w * b,
};
}
void mons_quat_mul_f_inplace(mons_quat *a, float b) {
a->x *= b;
a->y *= b;
a->z *= b;
a->w *= b;
}
mons_quat mons_quat_mul_i(mons_quat a, int b) {
return (mons_quat){
a.x * b,
a.y * b,
a.z * b,
a.w * b,
};
}
void mons_quat_mul_i_inplace(mons_quat *a, int b) {
a->x *= b;
a->y *= b;
a->z *= b;
a->w *= b;
}
mons_mat4 mons_quat_to_mat4(mons_quat a) {
return (mons_mat4){
{
1.0 - (2.0 * a.y * a.y) - (2.0 * a.z * a.z),
(2.0 * a.x * a.y) - (2.0 * a.w * a.z),
(2.0 * a.x * a.z) + (2.0 * a.w * a.y),
0.0,
},
{
(2.0 * a.x * a.y) + (2.0 * a.w * a.z),
1.0 - (2.0 * a.x * a.x) - (2.0 * a.z * a.z),
(2.0 * a.y * a.z) - (2.0 * a.w * a.x),
0.0,
},
{
(2.0 * a.x * a.z) - (2.0 * a.w * a.y),
(2.0 * a.y * a.z) + (2.0 * a.w * a.x),
1.0 - (2.0 * a.x * a.x) - (2.0 * a.y * a.y),
0.0,
},
{
0.0,
0.0,
0.0,
1.0,
},
};
}
mons_quat mons_quat_normalize(mons_quat a) {
mons_vec4 normalized = mons_vec4_normalize(*((mons_vec4 *)&a));
return *((mons_quat *)&normalized);
}
void mons_quat_normalize_inplace(mons_quat *a) {
mons_vec4_normalize_inplace((mons_vec4 *)a);
}
mons_vec3 mons_quat_transform_vec3(mons_quat quat, mons_vec3 vec) {
mons_vec3 quat_vec = (mons_vec3){quat.x, quat.y, quat.z};
float s = quat.w;
return mons_vec3_add(
mons_vec3_add(
mons_vec3_mul_f(quat_vec, mons_vec3_dot(quat_vec, vec) * 2.0f),
mons_vec3_mul_f(vec, s * s - mons_vec3_dot(quat_vec, quat_vec))),
mons_vec3_mul_f(mons_vec3_cross(quat_vec, vec), 2.0f * s));
}
mons_quat mons_quat_conjugate(mons_quat quat) {
return (mons_quat){
.x = -quat.x,
.y = -quat.y,
.z = -quat.z,
.w = quat.w,
};
}
mons_quat mons_quat_looking_at(mons_vec3 eye, mons_vec3 target, mons_vec3 up) {
mons_vec3 f = mons_vec3_normalize(mons_vec3_sub(target, eye));
mons_vec3 side = mons_vec3_normalize(mons_vec3_cross(up, f));
mons_vec3 b = mons_vec3_normalize(mons_vec3_add(f, MONS_VEC3_NEG_Z));
mons_quat p = mons_quat_from_axis_angle(mons_vec3_cross(b, f), mons_vec3_dot(b,f));
mons_vec3 r = (mons_vec3) {
p.w*p.w + p.x*p.x - p.y*p.y - p.z*p.z,
(2.0f * p.x * p.y) - (2.0f * p.w * p.z),
(2.0f * p.x * p.z) + (2.0f * p.w * p.y),
};
b = mons_vec3_normalize(mons_vec3_add(side, r));
mons_quat q = mons_quat_from_axis_angle(mons_vec3_cross(b,side), mons_vec3_dot(b, side));
return mons_quat_mul(p, q);
}
void mons_quat_print(mons_quat quat) {
mons_vec4_print(*(mons_vec4*)&quat);
}
|
