2 files changed, 15 insertions, 0 deletions
diff --git a/src/math/math.h b/src/math/math.h
index 8fa748b..b520f9e 100644
--- a/src/math/math.h
+++ b/src/math/math.h
@@ -81,6 +81,8 @@ float internal_vec3_magnitude2(Vec3* v1);
 void internal_vec3_lerp(Vec3* v1, Vec3* v2, float t, Vec3* out);
 void internal_vec3_slerp(Vec3* v1, Vec3* v2, float t, Vec3* out);
 
+void internal_vec3_orthogonalize(Vec3* x, Vec3* y, Vec3* z, Vec3* out_x, Vec3* out_y, Vec3* out_z); // 以z为基准进行正交化
+
 void internal_vec4_dividew(Vec4* v, Vec3* out);
 void internal_vec4_dividewnoz(Vec4* v, Vec4* out);
 
diff --git a/src/math/vec3.c b/src/math/vec3.c
index e9c12a4..43154dc 100644
--- a/src/math/vec3.c
+++ b/src/math/vec3.c
@@ -4,6 +4,8 @@
 #include "../util/assert.h"
 #include "../core/mem.h"
 
+#include "../shaders/common/mathlib.h"
+
 Vec3 vec3forward = {0,0,1};
 Vec3 vec3up = {0, 1, 0};
 Vec3 vec3left = {1, 0, 0};
@@ -134,4 +136,15 @@ void internal_vec3_minus(Vec3* v1, Vec3* v2, Vec3* out) {
 	out->x = v1->x - v2->x;
 	out->y = v1->y - v2->y;
 	out->z = v1->z - v2->z;
+}
+
+// x tangent
+// y bitangent
+// z normal
+void internal_vec3_orthogonalize(Vec3* x, Vec3* y, Vec3* z, Vec3* out_x, Vec3* out_y, Vec3* out_z) {
+	*out_z = *z; 
+	*out_x = vec3_minus(*x, vec3_scale(*z, vec3_dot(*z, *x)));
+	Vec3 v1 = vec3_scale(*z, vec3_dot(*z, *y));
+	Vec3 v2 = vec3_scale(*out_x, vec3_dot(*out_x, *y));
+	*out_y = vec3_minus(*y, vec3_plus(v1, v2));
 }
 \ No newline at end of file