1 files changed, 120 insertions, 0 deletions
diff --git a/src/math/vec3.c b/src/math/vec3.c
new file mode 100644
index 0000000..4d77b96
--- /dev/null
+++ b/src/math/vec3.c
@@ -0,0 +1,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;
+}
+\ No newline at end of file