1 files changed, 42 insertions, 21 deletions

diff --git a/core/math/vector3.cpp b/core/math/vector3.cpp
index b6965b3c32..f94f39b7f2 100644
--- a/core/math/vector3.cpp
+++ b/core/math/vector3.cpp
@@ -31,14 +31,17 @@
 #include "vector3.h"
 
 #include "core/math/basis.h"
+#include "core/math/vector2.h"
+#include "core/math/vector3i.h"
+#include "core/string/ustring.h"
 
-void Vector3::rotate(const Vector3 &p_axis, const real_t p_phi) {
-	*this = Basis(p_axis, p_phi).xform(*this);
+void Vector3::rotate(const Vector3 &p_axis, const real_t p_angle) {
+	*this = Basis(p_axis, p_angle).xform(*this);
 }
 
-Vector3 Vector3::rotated(const Vector3 &p_axis, const real_t p_phi) const {
+Vector3 Vector3::rotated(const Vector3 &p_axis, const real_t p_angle) const {
 	Vector3 r = *this;
-	r.rotate(p_axis, p_phi);
+	r.rotate(p_axis, p_angle);
 	return r;
 }
 
@@ -83,29 +86,43 @@ Vector3 Vector3::limit_length(const real_t p_len) const {
 }
 
 Vector3 Vector3::cubic_interpolate(const Vector3 &p_b, const Vector3 &p_pre_a, const Vector3 &p_post_b, const real_t p_weight) const {
-	Vector3 p0 = p_pre_a;
-	Vector3 p1 = *this;
-	Vector3 p2 = p_b;
-	Vector3 p3 = p_post_b;
-
-	real_t t = p_weight;
-	real_t t2 = t * t;
-	real_t t3 = t2 * t;
-
-	Vector3 out;
-	out = 0.5 *
-			((p1 * 2.0) +
-					(-p0 + p2) * t +
-					(2.0 * p0 - 5.0 * p1 + 4.0 * p2 - p3) * t2 +
-					(-p0 + 3.0 * p1 - 3.0 * p2 + p3) * t3);
-	return out;
+	Vector3 res = *this;
+	res.x = Math::cubic_interpolate(res.x, p_b.x, p_pre_a.x, p_post_b.x, p_weight);
+	res.y = Math::cubic_interpolate(res.y, p_b.y, p_pre_a.y, p_post_b.y, p_weight);
+	res.z = Math::cubic_interpolate(res.z, p_b.z, p_pre_a.z, p_post_b.z, p_weight);
+	return res;
 }
 
 Vector3 Vector3::move_toward(const Vector3 &p_to, const real_t p_delta) const {
 	Vector3 v = *this;
 	Vector3 vd = p_to - v;
 	real_t len = vd.length();
-	return len <= p_delta || len < CMP_EPSILON ? p_to : v + vd / len * p_delta;
+	return len <= p_delta || len < (real_t)CMP_EPSILON ? p_to : v + vd / len * p_delta;
+}
+
+Vector2 Vector3::octahedron_encode() const {
+	Vector3 n = *this;
+	n /= Math::abs(n.x) + Math::abs(n.y) + Math::abs(n.z);
+	Vector2 o;
+	if (n.z >= 0.0f) {
+		o.x = n.x;
+		o.y = n.y;
+	} else {
+		o.x = (1.0f - Math::abs(n.y)) * (n.x >= 0.0f ? 1.0f : -1.0f);
+		o.y = (1.0f - Math::abs(n.x)) * (n.y >= 0.0f ? 1.0f : -1.0f);
+	}
+	o.x = o.x * 0.5f + 0.5f;
+	o.y = o.y * 0.5f + 0.5f;
+	return o;
+}
+
+Vector3 Vector3::octahedron_decode(const Vector2 &p_oct) {
+	Vector2 f(p_oct.x * 2.0f - 1.0f, p_oct.y * 2.0f - 1.0f);
+	Vector3 n(f.x, f.y, 1.0f - Math::abs(f.x) - Math::abs(f.y));
+	float t = CLAMP(-n.z, 0.0f, 1.0f);
+	n.x += n.x >= 0 ? -t : t;
+	n.y += n.y >= 0 ? -t : t;
+	return n.normalized();
 }
 
 Basis Vector3::outer(const Vector3 &p_with) const {
@@ -123,3 +140,7 @@ bool Vector3::is_equal_approx(const Vector3 &p_v) const {
 Vector3::operator String() const {
 	return "(" + String::num_real(x, false) + ", " + String::num_real(y, false) + ", " + String::num_real(z, false) + ")";
 }
+
+Vector3::operator Vector3i() const {
+	return Vector3i(x, y, z);
+}