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
|
#include <math.h>
#include "math.h"
#include "../util/assert.h"
#include "../core/mem.h"
Vec3 vec3forward = {0,0,1};
Vec3 vec3up = {0, 1, 0};
Vec3 vec3left = {1, 0, 0};
void vec3_cross(Vec3* v1, Vec3* v2, Vec3* out) {
ssr_assert(v1 && v2 && out);
out->x = v1->y*v2->z - v1->z*v2->y;
out->y = v1->z*v2->x - v1->x*v2->z;
out->z = v1->x*v2->y - v1->y*v2->x;
}
void vec3_scale(Vec3* v, float k, Vec3* out) {
ssr_assert(v && out);
out->x = v->x * k;
out->y = v->y * k;
out->z = v->z * k;
}
void vec3_plus(Vec3* v1, Vec3* v2, Vec3* out) {
ssr_assert(v1 && v2 && out);
out->x = v1->x + v2->x;
out->y = v1->y + v2->y;
out->z = v1->z + v2->z;
}
void vec3_offset(Vec3* v, float offset, Vec3* out) {
ssr_assert(v && out);
out->x = v->x + offset;
out->y = v->y + offset;
out->z = v->z + offset;
}
float vec3_magnitude(Vec3* v) {
ssr_assert(v);
float l2 = vec3_magnitude2(v);
return sqrt(l2);
}
float vec3_magnitude2(Vec3* v) {
ssr_assert(v);
return v->x*v->x + v->y*v->y + v->z*v->z;
}
void vec3_lerp(Vec3* v1, Vec3* v2, float t, Vec3* out) {
ssr_assert(v1 && v2 && out);
float t2 = 1 - t;
out->x = v1->x * t2 + v2->x * t;
out->y = v1->y * t2 + v2->y * t;
out->z = v1->z * t2 + v2->z * t;
}
void vec3_slerp(Vec3* v1, Vec3* v2, float t, Vec3* out) {
ssr_assert(v1 && v2 && out);
float mag1 = vec3_magnitude(v1);
float mag2 = vec3_magnitude(v2);
if (mag1 < EPSILON || mag2 < EPSILON) {
vec3_lerp(v1, v2, t, out);
return;
}
float lerplen = lerp(mag1, mag2, t);
/*分为长度上的lerp和角度上的slerp*/
float dot = vec3_dot(v1, v2) / (mag1 * mag2);
if (dot > 1.f - EPSILON) { /*几乎是同向*/
vec3_lerp(v1, v2, t, out);
return;
} else if (dot < -1 + EPSILON) { /*几乎是反向*/
} else {
Vec3 axis; vec3_cross(v1, v2, &axis); vec3_normalize(&axis, &axis);
Vec3 v1n; vec3_normalize(v1, &v1n);
float angle = acos(dot) * t;
//Mat4 m; mat4_setaxisangle(&axis, angle, &m);
//mat4_applytovec4(&m, &v1n, &v1n);
Quat q; quat_fromaxisangle(&axis, angle, &q);
quat_applytovec3(&q, &v1n, &v1n);
vec3_scale(&v1n, lerplen, out);
}
}
float vec3_dot(Vec3* v1, Vec3* v2) {
ssr_assert(v1 && v2);
return v1->x * v2->x + v1->y * v2->y + v1->z * v2->z;
}
void vec3_multiply(Vec3* v1, Vec3* v2, Quat* out) {
ssr_assert(v1 && v2 && out);
vec3_cross(v1, v2, out);
out->w = -vec3_dot(v1, v2);
}
void vec3_normalize(Vec3* v, Vec3* out) {
ssr_assert(v && out);
//float mag = rsqrt(v->x * v->x + v->y * v->y + v->z * v->z);
float mag = 1.f / vec3_magnitude(v);
out->x = v->x * mag;
out->y = v->y * mag;
out->z = v->z * mag;
}
void vec3_tostring(Vec3* v, char buf[]) {
sprintf(buf, "%8.3f %8.3f %8.3f", v->x, v->y, v->z);
}
void vec3_print(Vec3* v) {
vec3_tostring(v, printbuffer);
printf("\n%s\n", printbuffer);
}
void vec3_minus(Vec3* v1, Vec3* v2, Vec3* out) {
ssr_assert(v1 && v2 && out);
out->x = v1->x - v2->x;
out->y = v1->y - v2->y;
out->z = v1->z - v2->z;
}
|
