2 files changed, 32 insertions, 11 deletions

diff --git a/core/math/basis.cpp b/core/math/basis.cpp
index 9fcecd1ba6..1540bc8fe1 100644
--- a/core/math/basis.cpp
+++ b/core/math/basis.cpp
@@ -813,21 +813,28 @@ void Basis::set_axis_angle(const Vector3 &p_axis, real_t p_phi) {
 	ERR_FAIL_COND(!p_axis.is_normalized());
 #endif
 	Vector3 axis_sq(p_axis.x * p_axis.x, p_axis.y * p_axis.y, p_axis.z * p_axis.z);
-
 	real_t cosine = Math::cos(p_phi);
-	real_t sine = Math::sin(p_phi);
-
 	elements[0][0] = axis_sq.x + cosine * (1.0 - axis_sq.x);
-	elements[0][1] = p_axis.x * p_axis.y * (1.0 - cosine) - p_axis.z * sine;
-	elements[0][2] = p_axis.z * p_axis.x * (1.0 - cosine) + p_axis.y * sine;
-
-	elements[1][0] = p_axis.x * p_axis.y * (1.0 - cosine) + p_axis.z * sine;
 	elements[1][1] = axis_sq.y + cosine * (1.0 - axis_sq.y);
-	elements[1][2] = p_axis.y * p_axis.z * (1.0 - cosine) - p_axis.x * sine;
-
-	elements[2][0] = p_axis.z * p_axis.x * (1.0 - cosine) - p_axis.y * sine;
-	elements[2][1] = p_axis.y * p_axis.z * (1.0 - cosine) + p_axis.x * sine;
 	elements[2][2] = axis_sq.z + cosine * (1.0 - axis_sq.z);
+
+	real_t sine = Math::sin(p_phi);
+	real_t t = 1 - cosine;
+
+	real_t xyzt = p_axis.x * p_axis.y * t;
+	real_t zyxs = p_axis.z * sine;
+	elements[0][1] = xyzt - zyxs;
+	elements[1][0] = xyzt + zyxs;
+
+	xyzt = p_axis.x * p_axis.z * t;
+	zyxs = p_axis.y * sine;
+	elements[0][2] = xyzt + zyxs;
+	elements[2][0] = xyzt - zyxs;
+
+	xyzt = p_axis.y * p_axis.z * t;
+	zyxs = p_axis.x * sine;
+	elements[1][2] = xyzt - zyxs;
+	elements[2][1] = xyzt + zyxs;
 }
 
 void Basis::set_axis_angle_scale(const Vector3 &p_axis, real_t p_phi, const Vector3 &p_scale) {
diff --git a/core/math/geometry.h b/core/math/geometry.h
index c62573dd13..0e144e491f 100644
--- a/core/math/geometry.h
+++ b/core/math/geometry.h
@@ -31,6 +31,7 @@
 #ifndef GEOMETRY_H
 #define GEOMETRY_H
 
+#include "core/math/delaunay.h"
 #include "core/math/face3.h"
 #include "core/math/rect2.h"
 #include "core/math/triangulate.h"
@@ -857,6 +858,19 @@ public:
 		return points;
 	}
 
+	static Vector<int> triangulate_delaunay_2d(const Vector<Vector2> &p_points) {
+
+		Vector<Delaunay2D::Triangle> tr = Delaunay2D::triangulate(p_points);
+		Vector<int> triangles;
+
+		for (int i = 0; i < tr.size(); i++) {
+			triangles.push_back(tr[i].points[0]);
+			triangles.push_back(tr[i].points[1]);
+			triangles.push_back(tr[i].points[2]);
+		}
+		return triangles;
+	}
+
 	static Vector<int> triangulate_polygon(const Vector<Vector2> &p_polygon) {
 
 		Vector<int> triangles;