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 internal_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 internal_vec3_make(float x, float y, float z) {
Vec3 v = {x, y, z};
return v;
}
void internal_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 internal_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 internal_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 internal_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 internal_vec3_magnitude(Vec3* v) {
ssr_assert(v);
float l2 = internal_vec3_magnitude2(v);
return sqrt(l2);
}
float internal_vec3_magnitude2(Vec3* v) {
ssr_assert(v);
return v->x*v->x + v->y*v->y + v->z*v->z;
}
void internal_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 internal_vec3_slerp(Vec3* v1, Vec3* v2, float t, Vec3* out) {
ssr_assert(v1 && v2 && out);
float mag1 = internal_vec3_magnitude(v1);
float mag2 = internal_vec3_magnitude(v2);
if (mag1 < EPSILON || mag2 < EPSILON) {
internal_vec3_lerp(v1, v2, t, out);
return;
}
float lerplen = lerp(mag1, mag2, t);
/*分为长度上的lerp和角度上的slerp*/
float dot = internal_vec3_dot(v1, v2) / (mag1 * mag2);
if (dot > 1.f - EPSILON) { /*几乎是同向*/
internal_vec3_lerp(v1, v2, t, out);
return;
} else if (dot < -1 + EPSILON) { /*几乎是反向*/
} else {
Vec3 axis; internal_vec3_cross(v1, v2, &axis); internal_vec3_normalize(&axis, &axis);
Vec3 v1n; internal_vec3_normalize(v1, &v1n);
float angle = acos(dot) * t;
//Mat4 m; internal_mat4_setaxisangle(&axis, angle, &m);
//internal_mat4_mulvec4(&m, &v1n, &v1n);
Quat q; internal_quat_fromaxisangle(&axis, angle, &q);
internal_quat_applytovec3(&q, &v1n, &v1n);
internal_vec3_scale(&v1n, lerplen, out);
}
}
float internal_vec3_dot(Vec3* v1, Vec3* v2) {
ssr_assert(v1 && v2);
return v1->x * v2->x + v1->y * v2->y + v1->z * v2->z;
}
void internal_vec3_multiply(Vec3* v1, Vec3* v2, Quat* out) {
ssr_assert(v1 && v2 && out);
internal_vec3_cross(v1, v2, out);
out->w = -internal_vec3_dot(v1, v2);
}
void internal_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 = internal_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 internal_vec3_tostring(Vec3* v, char buf[]) {
sprintf(buf, "%8.3f %8.3f %8.3f", v->x, v->y, v->z);
}
void internal_vec3_print(Vec3* v) {
internal_vec3_tostring(v, printbuffer);
printf("\n%s\n", printbuffer);
}
void internal_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;
}
|
