Merge pull request #50682 from aaronfranke/basis-looking-at

Move code for looking_at to Basis

1 files changed, 18 insertions, 0 deletions

diff --git a/core/math/basis.cpp b/core/math/basis.cpp
index aa3831d4cf..b5e25fb837 100644
--- a/core/math/basis.cpp
+++ b/core/math/basis.cpp
@@ -1129,3 +1129,21 @@ void Basis::rotate_sh(real_t *p_values) {
 	p_values[7] = -d3;
 	p_values[8] = d4 * s_scale_dst4;
 }
+
+Basis Basis::looking_at(const Vector3 &p_target, const Vector3 &p_up) {
+#ifdef MATH_CHECKS
+	ERR_FAIL_COND_V_MSG(p_target.is_equal_approx(Vector3()), Basis(), "The target vector can't be zero.");
+	ERR_FAIL_COND_V_MSG(p_up.is_equal_approx(Vector3()), Basis(), "The up vector can't be zero.");
+#endif
+	Vector3 v_z = -p_target.normalized();
+	Vector3 v_x = p_up.cross(v_z);
+#ifdef MATH_CHECKS
+	ERR_FAIL_COND_V_MSG(v_x.is_equal_approx(Vector3()), Basis(), "The target vector and up vector can't be parallel to each other.");
+#endif
+	v_x.normalize();
+	Vector3 v_y = v_z.cross(v_x);
+
+	Basis basis;
+	basis.set(v_x, v_y, v_z);
+	return basis;
+}