6 files changed, 27 insertions, 26 deletions
diff --git a/core/math/bvh_cull.inc b/core/math/bvh_cull.inc
index cba8ea6cb3..d7edc8a884 100644
--- a/core/math/bvh_cull.inc
+++ b/core/math/bvh_cull.inc
@@ -14,7 +14,7 @@ struct CullParams {
 	uint32_t pairable_type;
 
 	// optional components for different tests
-	Vector3 point;
+	Point point;
 	BVHABB_CLASS abb;
 	typename BVHABB_CLASS::ConvexHull hull;
 	typename BVHABB_CLASS::Segment segment;
diff --git a/core/math/bvh_debug.inc b/core/math/bvh_debug.inc
index a97304334c..55db794ee3 100644
--- a/core/math/bvh_debug.inc
+++ b/core/math/bvh_debug.inc
@@ -6,24 +6,21 @@ void _debug_recursive_print_tree(int p_tree_id) const {
 }
 
 String _debug_aabb_to_string(const BVHABB_CLASS &aabb) const {
-	String sz = "(";
-	sz += itos(aabb.min.x);
-	sz += " ~ ";
-	sz += itos(-aabb.neg_max.x);
-	sz += ") (";
+	Point size = aabb.calculate_size();
 
-	sz += itos(aabb.min.y);
-	sz += " ~ ";
-	sz += itos(-aabb.neg_max.y);
-	sz += ") (";
+	String sz;
+	float vol = 0.0;
 
-	sz += itos(aabb.min.z);
-	sz += " ~ ";
-	sz += itos(-aabb.neg_max.z);
-	sz += ") ";
+	for (int i = 0; i < Point::AXES_COUNT; ++i) {
+		sz += "(";
+		sz += itos(aabb.min[i]);
+		sz += " ~ ";
+		sz += itos(-aabb.neg_max[i]);
+		sz += ") ";
+
+		vol += size[i];
+	}
 
-	Vector3 size = aabb.calculate_size();
-	float vol = size.x * size.y * size.z;
 	sz += "vol " + itos(vol);
 
 	return sz;
diff --git a/core/math/bvh_split.inc b/core/math/bvh_split.inc
index 3fcc4c7b10..6f54d06ce7 100644
--- a/core/math/bvh_split.inc
+++ b/core/math/bvh_split.inc
@@ -28,11 +28,15 @@ void _split_leaf_sort_groups_simple(int &num_a, int &num_b, uint16_t *group_a, u
 	Point centre = full_bound.calculate_centre();
 	Point size = full_bound.calculate_size();
 
-	int order[3];
+	int order[Point::AXIS_COUNT];
 
 	order[0] = size.min_axis();
-	order[2] = size.max_axis();
-	order[1] = 3 - (order[0] + order[2]);
+	order[Point::AXIS_COUNT - 1] = size.max_axis();
+
+	static_assert(Point::AXIS_COUNT <= 3);
+	if (Point::AXIS_COUNT == 3) {
+		order[1] = 3 - (order[0] + order[2]);
+	}
 
 	// simplest case, split on the longest axis
 	int split_axis = order[0];
@@ -54,7 +58,7 @@ void _split_leaf_sort_groups_simple(int &num_a, int &num_b, uint16_t *group_a, u
 
 	// detect when split on longest axis failed
 	int min_threshold = MAX_ITEMS / 4;
-	int min_group_size[3];
+	int min_group_size[Point::AXIS_COUNT];
 	min_group_size[0] = MIN(num_a, num_b);
 	if (min_group_size[0] < min_threshold) {
 		// slow but sure .. first move everything back into a
@@ -64,7 +68,7 @@ void _split_leaf_sort_groups_simple(int &num_a, int &num_b, uint16_t *group_a, u
 		num_b = 0;
 
 		// now calculate the best split
-		for (int axis = 1; axis < 3; axis++) {
+		for (int axis = 1; axis < Point::AXIS_COUNT; axis++) {
 			split_axis = order[axis];
 			int count = 0;
 
@@ -82,7 +86,7 @@ void _split_leaf_sort_groups_simple(int &num_a, int &num_b, uint16_t *group_a, u
 		// best axis
 		int best_axis = 0;
 		int best_min = min_group_size[0];
-		for (int axis = 1; axis < 3; axis++) {
+		for (int axis = 1; axis < Point::AXIS_COUNT; axis++) {
 			if (min_group_size[axis] > best_min) {
 				best_min = min_group_size[axis];
 				best_axis = axis;
diff --git a/core/math/face3.cpp b/core/math/face3.cpp
index 9af3f868d2..045ab67ce8 100644
--- a/core/math/face3.cpp
+++ b/core/math/face3.cpp
@@ -151,8 +151,8 @@ Face3::Side Face3::get_side_of(const Face3 &p_face, ClockDirection p_clock_dir)
 }
 
 Vector3 Face3::get_random_point_inside() const {
-	real_t a = Math::random(0, 1);
-	real_t b = Math::random(0, 1);
+	real_t a = Math::random(0.0, 1.0);
+	real_t b = Math::random(0.0, 1.0);
 	if (a > b) {
 		SWAP(a, b);
 	}
diff --git a/core/math/transform_2d.cpp b/core/math/transform_2d.cpp
index 060b619892..496a557844 100644
--- a/core/math/transform_2d.cpp
+++ b/core/math/transform_2d.cpp
@@ -279,7 +279,7 @@ Transform2D Transform2D::interpolate_with(const Transform2D &p_transform, const
 	}
 
 	//construct matrix
-	Transform2D res(Math::atan2(v.y, v.x), p1.lerp(p2, p_c));
+	Transform2D res(v.angle(), p1.lerp(p2, p_c));
 	res.scale_basis(s1.lerp(s2, p_c));
 	return res;
 }
diff --git a/core/math/vector2.cpp b/core/math/vector2.cpp
index b53dc05a00..16e43d7d06 100644
--- a/core/math/vector2.cpp
+++ b/core/math/vector2.cpp
@@ -79,7 +79,7 @@ real_t Vector2::angle_to(const Vector2 &p_vector2) const {
 }
 
 real_t Vector2::angle_to_point(const Vector2 &p_vector2) const {
-	return Math::atan2(y - p_vector2.y, x - p_vector2.x);
+	return (*this - p_vector2).angle();
 }
 
 real_t Vector2::dot(const Vector2 &p_other) const {