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
|
#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};
Vec3 vec3zero = {0, 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;
}
Vec3 vec3_make(float x, float y, float z) {
Vec3 v = {x, y, z};
return v;
}
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_scale3(Vec3* v, Vec3* scalar, Vec3* out) {
ssr_assert(v && scalar && out);
out->x = v->x * scalar->x;
out->y = v->y * scalar->y;
out->z = v->z * scalar->z;
}
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_mulvec4(&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 = vec3_magnitude(v);
if (compare(mag, 0)) {
return;
}
mag = 1.f / mag;
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;
}
|
