initial commit, 4.5 stable

core/math/SCsub

@@ -0,0 +1,8 @@
+#!/usr/bin/env python
+from misc.utility.scons_hints import *
+
+Import("env")
+
+env_math = env.Clone()
+
+env_math.add_source_files(env.core_sources, "*.cpp")

core/math/a_star.compat.inc

@@ -0,0 +1,59 @@
+/**************************************************************************/
+/*  a_star.compat.inc                                                     */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#ifndef DISABLE_DEPRECATED
+
+Vector<int64_t> AStar3D::_get_id_path_bind_compat_88047(int64_t p_from_id, int64_t p_to_id) {
+	return get_id_path(p_from_id, p_to_id, false);
+}
+
+Vector<Vector3> AStar3D::_get_point_path_bind_compat_88047(int64_t p_from_id, int64_t p_to_id) {
+	return get_point_path(p_from_id, p_to_id, false);
+}
+
+void AStar3D::_bind_compatibility_methods() {
+	ClassDB::bind_compatibility_method(D_METHOD("get_id_path", "from_id", "to_id"), &AStar3D::_get_id_path_bind_compat_88047);
+	ClassDB::bind_compatibility_method(D_METHOD("get_point_path", "from_id", "to_id"), &AStar3D::_get_point_path_bind_compat_88047);
+}
+
+Vector<int64_t> AStar2D::_get_id_path_bind_compat_88047(int64_t p_from_id, int64_t p_to_id) {
+	return get_id_path(p_from_id, p_to_id, false);
+}
+
+Vector<Vector2> AStar2D::_get_point_path_bind_compat_88047(int64_t p_from_id, int64_t p_to_id) {
+	return get_point_path(p_from_id, p_to_id, false);
+}
+
+void AStar2D::_bind_compatibility_methods() {
+	ClassDB::bind_compatibility_method(D_METHOD("get_id_path", "from_id", "to_id"), &AStar2D::_get_id_path_bind_compat_88047);
+	ClassDB::bind_compatibility_method(D_METHOD("get_point_path", "from_id", "to_id"), &AStar2D::_get_point_path_bind_compat_88047);
+}
+
+#endif // DISABLE_DEPRECATED

core/math/a_star.cpp

@@ -0,0 +1,960 @@
+/**************************************************************************/
+/*  a_star.cpp                                                            */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "a_star.h"
+#include "a_star.compat.inc"
+
+#include "core/math/geometry_3d.h"
+
+int64_t AStar3D::get_available_point_id() const {
+	if (points.has(last_free_id)) {
+		int64_t cur_new_id = last_free_id + 1;
+		while (points.has(cur_new_id)) {
+			cur_new_id++;
+		}
+		last_free_id = cur_new_id;
+	}
+
+	return last_free_id;
+}
+
+void AStar3D::add_point(int64_t p_id, const Vector3 &p_pos, real_t p_weight_scale) {
+	ERR_FAIL_COND_MSG(p_id < 0, vformat("Can't add a point with negative id: %d.", p_id));
+	ERR_FAIL_COND_MSG(p_weight_scale < 0.0, vformat("Can't add a point with weight scale less than 0.0: %f.", p_weight_scale));
+
+	Point **point_entry = points.getptr(p_id);
+
+	if (!point_entry) {
+		Point *pt = memnew(Point);
+		pt->id = p_id;
+		pt->pos = p_pos;
+		pt->weight_scale = p_weight_scale;
+		pt->prev_point = nullptr;
+		pt->open_pass = 0;
+		pt->closed_pass = 0;
+		pt->enabled = true;
+		points.insert_new(p_id, pt);
+	} else {
+		Point *found_pt = *point_entry;
+		found_pt->pos = p_pos;
+		found_pt->weight_scale = p_weight_scale;
+	}
+}
+
+Vector3 AStar3D::get_point_position(int64_t p_id) const {
+	Point *const *point_entry = points.getptr(p_id);
+	ERR_FAIL_COND_V_MSG(!point_entry, Vector3(), vformat("Can't get point's position. Point with id: %d doesn't exist.", p_id));
+
+	return (*point_entry)->pos;
+}
+
+void AStar3D::set_point_position(int64_t p_id, const Vector3 &p_pos) {
+	Point **point_entry = points.getptr(p_id);
+	ERR_FAIL_COND_MSG(!point_entry, vformat("Can't set point's position. Point with id: %d doesn't exist.", p_id));
+
+	(*point_entry)->pos = p_pos;
+}
+
+real_t AStar3D::get_point_weight_scale(int64_t p_id) const {
+	Point *const *point_entry = points.getptr(p_id);
+	ERR_FAIL_COND_V_MSG(!point_entry, 0, vformat("Can't get point's weight scale. Point with id: %d doesn't exist.", p_id));
+
+	return (*point_entry)->weight_scale;
+}
+
+void AStar3D::set_point_weight_scale(int64_t p_id, real_t p_weight_scale) {
+	Point **point_entry = points.getptr(p_id);
+	ERR_FAIL_COND_MSG(!point_entry, vformat("Can't set point's weight scale. Point with id: %d doesn't exist.", p_id));
+	ERR_FAIL_COND_MSG(p_weight_scale < 0.0, vformat("Can't set point's weight scale less than 0.0: %f.", p_weight_scale));
+
+	(*point_entry)->weight_scale = p_weight_scale;
+}
+
+void AStar3D::remove_point(int64_t p_id) {
+	Point **point_entry = points.getptr(p_id);
+	ERR_FAIL_COND_MSG(!point_entry, vformat("Can't remove point. Point with id: %d doesn't exist.", p_id));
+	Point *p = *point_entry;
+
+	for (KeyValue<int64_t, Point *> &kv : p->neighbors) {
+		Segment s(p_id, kv.key);
+		segments.erase(s);
+
+		kv.value->neighbors.erase(p->id);
+		kv.value->unlinked_neighbours.erase(p->id);
+	}
+
+	for (KeyValue<int64_t, Point *> &kv : p->unlinked_neighbours) {
+		Segment s(p_id, kv.key);
+		segments.erase(s);
+
+		kv.value->neighbors.erase(p->id);
+		kv.value->unlinked_neighbours.erase(p->id);
+	}
+
+	memdelete(p);
+	points.erase(p_id);
+	last_free_id = p_id;
+}
+
+void AStar3D::connect_points(int64_t p_id, int64_t p_with_id, bool bidirectional) {
+	ERR_FAIL_COND_MSG(p_id == p_with_id, vformat("Can't connect point with id: %d to itself.", p_id));
+
+	Point **a_entry = points.getptr(p_id);
+	ERR_FAIL_COND_MSG(!a_entry, vformat("Can't connect points. Point with id: %d doesn't exist.", p_id));
+	Point *a = *a_entry;
+
+	Point **b_entry = points.getptr(p_with_id);
+	ERR_FAIL_COND_MSG(!b_entry, vformat("Can't connect points. Point with id: %d doesn't exist.", p_with_id));
+	Point *b = *b_entry;
+
+	a->neighbors.insert(b->id, b);
+
+	if (bidirectional) {
+		b->neighbors.insert(a->id, a);
+	} else {
+		b->unlinked_neighbours.insert(a->id, a);
+	}
+
+	Segment s(p_id, p_with_id);
+	if (bidirectional) {
+		s.direction = Segment::BIDIRECTIONAL;
+	}
+
+	HashSet<Segment, Segment>::Iterator element = segments.find(s);
+	if (element) {
+		s.direction |= element->direction;
+		if (s.direction == Segment::BIDIRECTIONAL) {
+			// Both are neighbors of each other now
+			a->unlinked_neighbours.erase(b->id);
+			b->unlinked_neighbours.erase(a->id);
+		}
+		segments.remove(element);
+	}
+
+	segments.insert(s);
+}
+
+void AStar3D::disconnect_points(int64_t p_id, int64_t p_with_id, bool bidirectional) {
+	Point **a_entry = points.getptr(p_id);
+	ERR_FAIL_COND_MSG(!a_entry, vformat("Can't disconnect points. Point with id: %d doesn't exist.", p_id));
+	Point *a = *a_entry;
+
+	Point **b_entry = points.getptr(p_with_id);
+	ERR_FAIL_COND_MSG(!b_entry, vformat("Can't disconnect points. Point with id: %d doesn't exist.", p_with_id));
+	Point *b = *b_entry;
+
+	Segment s(p_id, p_with_id);
+	int remove_direction = bidirectional ? (int)Segment::BIDIRECTIONAL : (int)s.direction;
+
+	HashSet<Segment, Segment>::Iterator element = segments.find(s);
+	if (element) {
+		// s is the new segment
+		// Erase the directions to be removed
+		s.direction = (element->direction & ~remove_direction);
+
+		a->neighbors.erase(b->id);
+		if (bidirectional) {
+			b->neighbors.erase(a->id);
+			if (element->direction != Segment::BIDIRECTIONAL) {
+				a->unlinked_neighbours.erase(b->id);
+				b->unlinked_neighbours.erase(a->id);
+			}
+		} else {
+			if (s.direction == Segment::NONE) {
+				b->unlinked_neighbours.erase(a->id);
+			} else {
+				a->unlinked_neighbours.insert(b->id, b);
+			}
+		}
+
+		segments.remove(element);
+		if (s.direction != Segment::NONE) {
+			segments.insert(s);
+		}
+	}
+}
+
+bool AStar3D::has_point(int64_t p_id) const {
+	return points.has(p_id);
+}
+
+PackedInt64Array AStar3D::get_point_ids() {
+	PackedInt64Array point_list;
+
+	for (KeyValue<int64_t, Point *> &kv : points) {
+		point_list.push_back(kv.key);
+	}
+
+	return point_list;
+}
+
+Vector<int64_t> AStar3D::get_point_connections(int64_t p_id) {
+	Point **p_entry = points.getptr(p_id);
+	ERR_FAIL_COND_V_MSG(!p_entry, Vector<int64_t>(), vformat("Can't get point's connections. Point with id: %d doesn't exist.", p_id));
+
+	Vector<int64_t> point_list;
+
+	for (KeyValue<int64_t, Point *> &kv : (*p_entry)->neighbors) {
+		point_list.push_back(kv.key);
+	}
+
+	return point_list;
+}
+
+bool AStar3D::are_points_connected(int64_t p_id, int64_t p_with_id, bool bidirectional) const {
+	Segment s(p_id, p_with_id);
+	const HashSet<Segment, Segment>::Iterator element = segments.find(s);
+
+	return element &&
+			(bidirectional || (element->direction & s.direction) == s.direction);
+}
+
+void AStar3D::clear() {
+	last_free_id = 0;
+	for (KeyValue<int64_t, Point *> &kv : points) {
+		memdelete(kv.value);
+	}
+	segments.clear();
+	points.clear();
+}
+
+int64_t AStar3D::get_point_count() const {
+	return points.size();
+}
+
+int64_t AStar3D::get_point_capacity() const {
+	return points.get_capacity();
+}
+
+void AStar3D::reserve_space(int64_t p_num_nodes) {
+	ERR_FAIL_COND_MSG(p_num_nodes <= 0, vformat("New capacity must be greater than 0, new was: %d.", p_num_nodes));
+	points.reserve(p_num_nodes);
+}
+
+int64_t AStar3D::get_closest_point(const Vector3 &p_point, bool p_include_disabled) const {
+	int64_t closest_id = -1;
+	real_t closest_dist = 1e20;
+
+	for (const KeyValue<int64_t, Point *> &kv : points) {
+		if (!p_include_disabled && !kv.value->enabled) {
+			continue; // Disabled points should not be considered.
+		}
+
+		// Keep the closest point's ID, and in case of multiple closest IDs,
+		// the smallest one (makes it deterministic).
+		real_t d = p_point.distance_squared_to(kv.value->pos);
+		int64_t id = kv.key;
+		if (d <= closest_dist) {
+			if (d == closest_dist && id > closest_id) { // Keep lowest ID.
+				continue;
+			}
+			closest_dist = d;
+			closest_id = id;
+		}
+	}
+
+	return closest_id;
+}
+
+Vector3 AStar3D::get_closest_position_in_segment(const Vector3 &p_point) const {
+	real_t closest_dist = 1e20;
+	Vector3 closest_point;
+
+	for (const Segment &E : segments) {
+		const Point *from_point = *points.getptr(E.key.first);
+		const Point *to_point = *points.getptr(E.key.second);
+
+		if (!(from_point->enabled && to_point->enabled)) {
+			continue;
+		}
+
+		Vector3 p = Geometry3D::get_closest_point_to_segment(p_point, from_point->pos, to_point->pos);
+		real_t d = p_point.distance_squared_to(p);
+		if (d < closest_dist) {
+			closest_point = p;
+			closest_dist = d;
+		}
+	}
+
+	return closest_point;
+}
+
+bool AStar3D::_solve(Point *begin_point, Point *end_point, bool p_allow_partial_path) {
+	last_closest_point = nullptr;
+	pass++;
+
+	if (!end_point->enabled && !p_allow_partial_path) {
+		return false;
+	}
+
+	bool found_route = false;
+
+	LocalVector<Point *> open_list;
+	SortArray<Point *, SortPoints> sorter;
+
+	begin_point->g_score = 0;
+	begin_point->f_score = _estimate_cost(begin_point->id, end_point->id);
+	begin_point->abs_g_score = 0;
+	begin_point->abs_f_score = _estimate_cost(begin_point->id, end_point->id);
+	open_list.push_back(begin_point);
+
+	while (!open_list.is_empty()) {
+		Point *p = open_list[0]; // The currently processed point.
+
+		// Find point closer to end_point, or same distance to end_point but closer to begin_point.
+		if (last_closest_point == nullptr || last_closest_point->abs_f_score > p->abs_f_score || (last_closest_point->abs_f_score >= p->abs_f_score && last_closest_point->abs_g_score > p->abs_g_score)) {
+			last_closest_point = p;
+		}
+
+		if (p == end_point) {
+			found_route = true;
+			break;
+		}
+
+		sorter.pop_heap(0, open_list.size(), open_list.ptr()); // Remove the current point from the open list.
+		open_list.remove_at(open_list.size() - 1);
+		p->closed_pass = pass; // Mark the point as closed.
+
+		for (const KeyValue<int64_t, Point *> &kv : p->neighbors) {
+			Point *e = kv.value; // The neighbor point.
+
+			if (!e->enabled || e->closed_pass == pass) {
+				continue;
+			}
+
+			if (neighbor_filter_enabled) {
+				bool filtered;
+				if (GDVIRTUAL_CALL(_filter_neighbor, p->id, e->id, filtered) && filtered) {
+					continue;
+				}
+			}
+
+			real_t tentative_g_score = p->g_score + _compute_cost(p->id, e->id) * e->weight_scale;
+
+			bool new_point = false;
+
+			if (e->open_pass != pass) { // The point wasn't inside the open list.
+				e->open_pass = pass;
+				open_list.push_back(e);
+				new_point = true;
+			} else if (tentative_g_score >= e->g_score) { // The new path is worse than the previous.
+				continue;
+			}
+
+			e->prev_point = p;
+			e->g_score = tentative_g_score;
+			e->f_score = e->g_score + _estimate_cost(e->id, end_point->id);
+			e->abs_g_score = tentative_g_score;
+			e->abs_f_score = e->f_score - e->g_score;
+
+			if (new_point) { // The position of the new points is already known.
+				sorter.push_heap(0, open_list.size() - 1, 0, e, open_list.ptr());
+			} else {
+				sorter.push_heap(0, open_list.find(e), 0, e, open_list.ptr());
+			}
+		}
+	}
+
+	return found_route;
+}
+
+real_t AStar3D::_estimate_cost(int64_t p_from_id, int64_t p_end_id) {
+	real_t scost;
+	if (GDVIRTUAL_CALL(_estimate_cost, p_from_id, p_end_id, scost)) {
+		return scost;
+	}
+
+	Point **from_entry = points.getptr(p_from_id);
+	ERR_FAIL_COND_V_MSG(!from_entry, 0, vformat("Can't estimate cost. Point with id: %d doesn't exist.", p_from_id));
+	Point *from_point = *from_entry;
+
+	Point **end_entry = points.getptr(p_end_id);
+	ERR_FAIL_COND_V_MSG(!end_entry, 0, vformat("Can't estimate cost. Point with id: %d doesn't exist.", p_end_id));
+	Point *end_point = *end_entry;
+
+	return from_point->pos.distance_to(end_point->pos);
+}
+
+real_t AStar3D::_compute_cost(int64_t p_from_id, int64_t p_to_id) {
+	real_t scost;
+	if (GDVIRTUAL_CALL(_compute_cost, p_from_id, p_to_id, scost)) {
+		return scost;
+	}
+
+	Point **from_entry = points.getptr(p_from_id);
+	ERR_FAIL_COND_V_MSG(!from_entry, 0, vformat("Can't compute cost. Point with id: %d doesn't exist.", p_from_id));
+	Point *from_point = *from_entry;
+
+	Point **to_entry = points.getptr(p_to_id);
+	ERR_FAIL_COND_V_MSG(!to_entry, 0, vformat("Can't compute cost. Point with id: %d doesn't exist.", p_to_id));
+	Point *to_point = *to_entry;
+
+	return from_point->pos.distance_to(to_point->pos);
+}
+
+Vector<Vector3> AStar3D::get_point_path(int64_t p_from_id, int64_t p_to_id, bool p_allow_partial_path) {
+	Point **a_entry = points.getptr(p_from_id);
+	ERR_FAIL_COND_V_MSG(!a_entry, Vector<Vector3>(), vformat("Can't get point path. Point with id: %d doesn't exist.", p_from_id));
+	Point *a = *a_entry;
+
+	Point **b_entry = points.getptr(p_to_id);
+	ERR_FAIL_COND_V_MSG(!b_entry, Vector<Vector3>(), vformat("Can't get point path. Point with id: %d doesn't exist.", p_to_id));
+	Point *b = *b_entry;
+
+	if (a == b) {
+		Vector<Vector3> ret;
+		ret.push_back(a->pos);
+		return ret;
+	}
+
+	Point *begin_point = a;
+	Point *end_point = b;
+
+	bool found_route = _solve(begin_point, end_point, p_allow_partial_path);
+	if (!found_route) {
+		if (!p_allow_partial_path || last_closest_point == nullptr) {
+			return Vector<Vector3>();
+		}
+
+		// Use closest point instead.
+		end_point = last_closest_point;
+	}
+
+	Point *p = end_point;
+	int64_t pc = 1; // Begin point
+	while (p != begin_point) {
+		pc++;
+		p = p->prev_point;
+	}
+
+	Vector<Vector3> path;
+	path.resize(pc);
+
+	{
+		Vector3 *w = path.ptrw();
+
+		Point *p2 = end_point;
+		int64_t idx = pc - 1;
+		while (p2 != begin_point) {
+			w[idx--] = p2->pos;
+			p2 = p2->prev_point;
+		}
+
+		w[0] = p2->pos; // Assign first
+	}
+
+	return path;
+}
+
+Vector<int64_t> AStar3D::get_id_path(int64_t p_from_id, int64_t p_to_id, bool p_allow_partial_path) {
+	Point **a_entry = points.getptr(p_from_id);
+	ERR_FAIL_COND_V_MSG(!a_entry, Vector<int64_t>(), vformat("Can't get id path. Point with id: %d doesn't exist.", p_from_id));
+	Point *a = *a_entry;
+
+	Point **b_entry = points.getptr(p_to_id);
+	ERR_FAIL_COND_V_MSG(!b_entry, Vector<int64_t>(), vformat("Can't get id path. Point with id: %d doesn't exist.", p_to_id));
+	Point *b = *b_entry;
+
+	if (a == b) {
+		Vector<int64_t> ret;
+		ret.push_back(a->id);
+		return ret;
+	}
+
+	if (!a->enabled) {
+		return Vector<int64_t>();
+	}
+
+	Point *begin_point = a;
+	Point *end_point = b;
+
+	bool found_route = _solve(begin_point, end_point, p_allow_partial_path);
+	if (!found_route) {
+		if (!p_allow_partial_path || last_closest_point == nullptr) {
+			return Vector<int64_t>();
+		}
+
+		// Use closest point instead.
+		end_point = last_closest_point;
+	}
+
+	Point *p = end_point;
+	int64_t pc = 1; // Begin point
+	while (p != begin_point) {
+		pc++;
+		p = p->prev_point;
+	}
+
+	Vector<int64_t> path;
+	path.resize(pc);
+
+	{
+		int64_t *w = path.ptrw();
+
+		p = end_point;
+		int64_t idx = pc - 1;
+		while (p != begin_point) {
+			w[idx--] = p->id;
+			p = p->prev_point;
+		}
+
+		w[0] = p->id; // Assign first
+	}
+
+	return path;
+}
+
+bool AStar3D::is_neighbor_filter_enabled() const {
+	return neighbor_filter_enabled;
+}
+
+void AStar3D::set_neighbor_filter_enabled(bool p_enabled) {
+	neighbor_filter_enabled = p_enabled;
+}
+
+void AStar3D::set_point_disabled(int64_t p_id, bool p_disabled) {
+	Point **p_entry = points.getptr(p_id);
+	ERR_FAIL_COND_MSG(!p_entry, vformat("Can't set if point is disabled. Point with id: %d doesn't exist.", p_id));
+	Point *p = *p_entry;
+
+	p->enabled = !p_disabled;
+}
+
+bool AStar3D::is_point_disabled(int64_t p_id) const {
+	Point *const *p_entry = points.getptr(p_id);
+	ERR_FAIL_COND_V_MSG(!p_entry, false, vformat("Can't get if point is disabled. Point with id: %d doesn't exist.", p_id));
+	Point *p = *p_entry;
+
+	return !p->enabled;
+}
+
+void AStar3D::_bind_methods() {
+	ClassDB::bind_method(D_METHOD("get_available_point_id"), &AStar3D::get_available_point_id);
+	ClassDB::bind_method(D_METHOD("add_point", "id", "position", "weight_scale"), &AStar3D::add_point, DEFVAL(1.0));
+	ClassDB::bind_method(D_METHOD("get_point_position", "id"), &AStar3D::get_point_position);
+	ClassDB::bind_method(D_METHOD("set_point_position", "id", "position"), &AStar3D::set_point_position);
+	ClassDB::bind_method(D_METHOD("get_point_weight_scale", "id"), &AStar3D::get_point_weight_scale);
+	ClassDB::bind_method(D_METHOD("set_point_weight_scale", "id", "weight_scale"), &AStar3D::set_point_weight_scale);
+	ClassDB::bind_method(D_METHOD("remove_point", "id"), &AStar3D::remove_point);
+	ClassDB::bind_method(D_METHOD("has_point", "id"), &AStar3D::has_point);
+	ClassDB::bind_method(D_METHOD("get_point_connections", "id"), &AStar3D::get_point_connections);
+	ClassDB::bind_method(D_METHOD("get_point_ids"), &AStar3D::get_point_ids);
+
+	ClassDB::bind_method(D_METHOD("set_point_disabled", "id", "disabled"), &AStar3D::set_point_disabled, DEFVAL(true));
+	ClassDB::bind_method(D_METHOD("is_point_disabled", "id"), &AStar3D::is_point_disabled);
+
+	ClassDB::bind_method(D_METHOD("set_neighbor_filter_enabled", "enabled"), &AStar3D::set_neighbor_filter_enabled);
+	ClassDB::bind_method(D_METHOD("is_neighbor_filter_enabled"), &AStar3D::is_neighbor_filter_enabled);
+
+	ClassDB::bind_method(D_METHOD("connect_points", "id", "to_id", "bidirectional"), &AStar3D::connect_points, DEFVAL(true));
+	ClassDB::bind_method(D_METHOD("disconnect_points", "id", "to_id", "bidirectional"), &AStar3D::disconnect_points, DEFVAL(true));
+	ClassDB::bind_method(D_METHOD("are_points_connected", "id", "to_id", "bidirectional"), &AStar3D::are_points_connected, DEFVAL(true));
+
+	ClassDB::bind_method(D_METHOD("get_point_count"), &AStar3D::get_point_count);
+	ClassDB::bind_method(D_METHOD("get_point_capacity"), &AStar3D::get_point_capacity);
+	ClassDB::bind_method(D_METHOD("reserve_space", "num_nodes"), &AStar3D::reserve_space);
+	ClassDB::bind_method(D_METHOD("clear"), &AStar3D::clear);
+
+	ClassDB::bind_method(D_METHOD("get_closest_point", "to_position", "include_disabled"), &AStar3D::get_closest_point, DEFVAL(false));
+	ClassDB::bind_method(D_METHOD("get_closest_position_in_segment", "to_position"), &AStar3D::get_closest_position_in_segment);
+
+	ClassDB::bind_method(D_METHOD("get_point_path", "from_id", "to_id", "allow_partial_path"), &AStar3D::get_point_path, DEFVAL(false));
+	ClassDB::bind_method(D_METHOD("get_id_path", "from_id", "to_id", "allow_partial_path"), &AStar3D::get_id_path, DEFVAL(false));
+
+	GDVIRTUAL_BIND(_filter_neighbor, "from_id", "neighbor_id")
+	GDVIRTUAL_BIND(_estimate_cost, "from_id", "end_id")
+	GDVIRTUAL_BIND(_compute_cost, "from_id", "to_id")
+
+	ADD_PROPERTY(PropertyInfo(Variant::BOOL, "neighbor_filter_enabled"), "set_neighbor_filter_enabled", "is_neighbor_filter_enabled");
+}
+
+AStar3D::~AStar3D() {
+	clear();
+}
+
+/////////////////////////////////////////////////////////////
+
+int64_t AStar2D::get_available_point_id() const {
+	return astar.get_available_point_id();
+}
+
+void AStar2D::add_point(int64_t p_id, const Vector2 &p_pos, real_t p_weight_scale) {
+	astar.add_point(p_id, Vector3(p_pos.x, p_pos.y, 0), p_weight_scale);
+}
+
+Vector2 AStar2D::get_point_position(int64_t p_id) const {
+	Vector3 p = astar.get_point_position(p_id);
+	return Vector2(p.x, p.y);
+}
+
+void AStar2D::set_point_position(int64_t p_id, const Vector2 &p_pos) {
+	astar.set_point_position(p_id, Vector3(p_pos.x, p_pos.y, 0));
+}
+
+real_t AStar2D::get_point_weight_scale(int64_t p_id) const {
+	return astar.get_point_weight_scale(p_id);
+}
+
+void AStar2D::set_point_weight_scale(int64_t p_id, real_t p_weight_scale) {
+	astar.set_point_weight_scale(p_id, p_weight_scale);
+}
+
+void AStar2D::remove_point(int64_t p_id) {
+	astar.remove_point(p_id);
+}
+
+bool AStar2D::has_point(int64_t p_id) const {
+	return astar.has_point(p_id);
+}
+
+Vector<int64_t> AStar2D::get_point_connections(int64_t p_id) {
+	return astar.get_point_connections(p_id);
+}
+
+PackedInt64Array AStar2D::get_point_ids() {
+	return astar.get_point_ids();
+}
+
+bool AStar2D::is_neighbor_filter_enabled() const {
+	return astar.neighbor_filter_enabled;
+}
+
+void AStar2D::set_neighbor_filter_enabled(bool p_enabled) {
+	astar.neighbor_filter_enabled = p_enabled;
+}
+
+void AStar2D::set_point_disabled(int64_t p_id, bool p_disabled) {
+	astar.set_point_disabled(p_id, p_disabled);
+}
+
+bool AStar2D::is_point_disabled(int64_t p_id) const {
+	return astar.is_point_disabled(p_id);
+}
+
+void AStar2D::connect_points(int64_t p_id, int64_t p_with_id, bool p_bidirectional) {
+	astar.connect_points(p_id, p_with_id, p_bidirectional);
+}
+
+void AStar2D::disconnect_points(int64_t p_id, int64_t p_with_id, bool p_bidirectional) {
+	astar.disconnect_points(p_id, p_with_id, p_bidirectional);
+}
+
+bool AStar2D::are_points_connected(int64_t p_id, int64_t p_with_id, bool p_bidirectional) const {
+	return astar.are_points_connected(p_id, p_with_id, p_bidirectional);
+}
+
+int64_t AStar2D::get_point_count() const {
+	return astar.get_point_count();
+}
+
+int64_t AStar2D::get_point_capacity() const {
+	return astar.get_point_capacity();
+}
+
+void AStar2D::clear() {
+	astar.clear();
+}
+
+void AStar2D::reserve_space(int64_t p_num_nodes) {
+	astar.reserve_space(p_num_nodes);
+}
+
+int64_t AStar2D::get_closest_point(const Vector2 &p_point, bool p_include_disabled) const {
+	return astar.get_closest_point(Vector3(p_point.x, p_point.y, 0), p_include_disabled);
+}
+
+Vector2 AStar2D::get_closest_position_in_segment(const Vector2 &p_point) const {
+	Vector3 p = astar.get_closest_position_in_segment(Vector3(p_point.x, p_point.y, 0));
+	return Vector2(p.x, p.y);
+}
+
+real_t AStar2D::_estimate_cost(int64_t p_from_id, int64_t p_end_id) {
+	real_t scost;
+	if (GDVIRTUAL_CALL(_estimate_cost, p_from_id, p_end_id, scost)) {
+		return scost;
+	}
+
+	AStar3D::Point **from_entry = astar.points.getptr(p_from_id);
+	ERR_FAIL_COND_V_MSG(!from_entry, 0, vformat("Can't estimate cost. Point with id: %d doesn't exist.", p_from_id));
+	AStar3D::Point *from_point = *from_entry;
+
+	AStar3D::Point **end_entry = astar.points.getptr(p_end_id);
+	ERR_FAIL_COND_V_MSG(!end_entry, 0, vformat("Can't estimate cost. Point with id: %d doesn't exist.", p_end_id));
+	AStar3D::Point *end_point = *end_entry;
+
+	return from_point->pos.distance_to(end_point->pos);
+}
+
+real_t AStar2D::_compute_cost(int64_t p_from_id, int64_t p_to_id) {
+	real_t scost;
+	if (GDVIRTUAL_CALL(_compute_cost, p_from_id, p_to_id, scost)) {
+		return scost;
+	}
+
+	AStar3D::Point **from_entry = astar.points.getptr(p_from_id);
+	ERR_FAIL_COND_V_MSG(!from_entry, 0, vformat("Can't compute cost. Point with id: %d doesn't exist.", p_from_id));
+	AStar3D::Point *from_point = *from_entry;
+
+	AStar3D::Point **to_entry = astar.points.getptr(p_to_id);
+	ERR_FAIL_COND_V_MSG(!to_entry, 0, vformat("Can't compute cost. Point with id: %d doesn't exist.", p_to_id));
+	AStar3D::Point *to_point = *to_entry;
+
+	return from_point->pos.distance_to(to_point->pos);
+}
+
+Vector<Vector2> AStar2D::get_point_path(int64_t p_from_id, int64_t p_to_id, bool p_allow_partial_path) {
+	AStar3D::Point **a_entry = astar.points.getptr(p_from_id);
+	ERR_FAIL_COND_V_MSG(!a_entry, Vector<Vector2>(), vformat("Can't get point path. Point with id: %d doesn't exist.", p_from_id));
+	AStar3D::Point *a = *a_entry;
+
+	AStar3D::Point **b_entry = astar.points.getptr(p_to_id);
+	ERR_FAIL_COND_V_MSG(!b_entry, Vector<Vector2>(), vformat("Can't get point path. Point with id: %d doesn't exist.", p_to_id));
+	AStar3D::Point *b = *b_entry;
+
+	if (a == b) {
+		Vector<Vector2> ret = { Vector2(a->pos.x, a->pos.y) };
+		return ret;
+	}
+
+	AStar3D::Point *begin_point = a;
+	AStar3D::Point *end_point = b;
+
+	bool found_route = _solve(begin_point, end_point, p_allow_partial_path);
+	if (!found_route) {
+		if (!p_allow_partial_path || astar.last_closest_point == nullptr) {
+			return Vector<Vector2>();
+		}
+
+		// Use closest point instead.
+		end_point = astar.last_closest_point;
+	}
+
+	AStar3D::Point *p = end_point;
+	int64_t pc = 1; // Begin point
+	while (p != begin_point) {
+		pc++;
+		p = p->prev_point;
+	}
+
+	Vector<Vector2> path;
+	path.resize(pc);
+
+	{
+		Vector2 *w = path.ptrw();
+
+		AStar3D::Point *p2 = end_point;
+		int64_t idx = pc - 1;
+		while (p2 != begin_point) {
+			w[idx--] = Vector2(p2->pos.x, p2->pos.y);
+			p2 = p2->prev_point;
+		}
+
+		w[0] = Vector2(p2->pos.x, p2->pos.y); // Assign first
+	}
+
+	return path;
+}
+
+Vector<int64_t> AStar2D::get_id_path(int64_t p_from_id, int64_t p_to_id, bool p_allow_partial_path) {
+	AStar3D::Point **a_entry = astar.points.getptr(p_from_id);
+	ERR_FAIL_COND_V_MSG(!a_entry, Vector<int64_t>(), vformat("Can't get id path. Point with id: %d doesn't exist.", p_from_id));
+	AStar3D::Point *a = *a_entry;
+
+	AStar3D::Point **to_entry = astar.points.getptr(p_to_id);
+	ERR_FAIL_COND_V_MSG(!to_entry, Vector<int64_t>(), vformat("Can't get id path. Point with id: %d doesn't exist.", p_to_id));
+	AStar3D::Point *b = *to_entry;
+
+	if (a == b) {
+		Vector<int64_t> ret;
+		ret.push_back(a->id);
+		return ret;
+	}
+
+	if (!a->enabled) {
+		return Vector<int64_t>();
+	}
+
+	AStar3D::Point *begin_point = a;
+	AStar3D::Point *end_point = b;
+
+	bool found_route = _solve(begin_point, end_point, p_allow_partial_path);
+	if (!found_route) {
+		if (!p_allow_partial_path || astar.last_closest_point == nullptr) {
+			return Vector<int64_t>();
+		}
+
+		// Use closest point instead.
+		end_point = astar.last_closest_point;
+	}
+
+	AStar3D::Point *p = end_point;
+	int64_t pc = 1; // Begin point
+	while (p != begin_point) {
+		pc++;
+		p = p->prev_point;
+	}
+
+	Vector<int64_t> path;
+	path.resize(pc);
+
+	{
+		int64_t *w = path.ptrw();
+
+		p = end_point;
+		int64_t idx = pc - 1;
+		while (p != begin_point) {
+			w[idx--] = p->id;
+			p = p->prev_point;
+		}
+
+		w[0] = p->id; // Assign first
+	}
+
+	return path;
+}
+
+bool AStar2D::_solve(AStar3D::Point *begin_point, AStar3D::Point *end_point, bool p_allow_partial_path) {
+	astar.last_closest_point = nullptr;
+	astar.pass++;
+
+	if (!end_point->enabled && !p_allow_partial_path) {
+		return false;
+	}
+
+	bool found_route = false;
+
+	LocalVector<AStar3D::Point *> open_list;
+	SortArray<AStar3D::Point *, AStar3D::SortPoints> sorter;
+
+	begin_point->g_score = 0;
+	begin_point->f_score = _estimate_cost(begin_point->id, end_point->id);
+	begin_point->abs_g_score = 0;
+	begin_point->abs_f_score = _estimate_cost(begin_point->id, end_point->id);
+	open_list.push_back(begin_point);
+
+	while (!open_list.is_empty()) {
+		AStar3D::Point *p = open_list[0]; // The currently processed point.
+
+		// Find point closer to end_point, or same distance to end_point but closer to begin_point.
+		if (astar.last_closest_point == nullptr || astar.last_closest_point->abs_f_score > p->abs_f_score || (astar.last_closest_point->abs_f_score >= p->abs_f_score && astar.last_closest_point->abs_g_score > p->abs_g_score)) {
+			astar.last_closest_point = p;
+		}
+
+		if (p == end_point) {
+			found_route = true;
+			break;
+		}
+
+		sorter.pop_heap(0, open_list.size(), open_list.ptr()); // Remove the current point from the open list.
+		open_list.remove_at(open_list.size() - 1);
+		p->closed_pass = astar.pass; // Mark the point as closed.
+
+		for (KeyValue<int64_t, AStar3D::Point *> &kv : p->neighbors) {
+			AStar3D::Point *e = kv.value; // The neighbor point.
+
+			if (!e->enabled || e->closed_pass == astar.pass) {
+				continue;
+			}
+
+			if (astar.neighbor_filter_enabled) {
+				bool filtered;
+				if (GDVIRTUAL_CALL(_filter_neighbor, p->id, e->id, filtered) && filtered) {
+					continue;
+				}
+			}
+
+			real_t tentative_g_score = p->g_score + _compute_cost(p->id, e->id) * e->weight_scale;
+
+			bool new_point = false;
+
+			if (e->open_pass != astar.pass) { // The point wasn't inside the open list.
+				e->open_pass = astar.pass;
+				open_list.push_back(e);
+				new_point = true;
+			} else if (tentative_g_score >= e->g_score) { // The new path is worse than the previous.
+				continue;
+			}
+
+			e->prev_point = p;
+			e->g_score = tentative_g_score;
+			e->f_score = e->g_score + _estimate_cost(e->id, end_point->id);
+			e->abs_g_score = tentative_g_score;
+			e->abs_f_score = e->f_score - e->g_score;
+
+			if (new_point) { // The position of the new points is already known.
+				sorter.push_heap(0, open_list.size() - 1, 0, e, open_list.ptr());
+			} else {
+				sorter.push_heap(0, open_list.find(e), 0, e, open_list.ptr());
+			}
+		}
+	}
+
+	return found_route;
+}
+
+void AStar2D::_bind_methods() {
+	ClassDB::bind_method(D_METHOD("get_available_point_id"), &AStar2D::get_available_point_id);
+	ClassDB::bind_method(D_METHOD("add_point", "id", "position", "weight_scale"), &AStar2D::add_point, DEFVAL(1.0));
+	ClassDB::bind_method(D_METHOD("get_point_position", "id"), &AStar2D::get_point_position);
+	ClassDB::bind_method(D_METHOD("set_point_position", "id", "position"), &AStar2D::set_point_position);
+	ClassDB::bind_method(D_METHOD("get_point_weight_scale", "id"), &AStar2D::get_point_weight_scale);
+	ClassDB::bind_method(D_METHOD("set_point_weight_scale", "id", "weight_scale"), &AStar2D::set_point_weight_scale);
+	ClassDB::bind_method(D_METHOD("remove_point", "id"), &AStar2D::remove_point);
+	ClassDB::bind_method(D_METHOD("has_point", "id"), &AStar2D::has_point);
+	ClassDB::bind_method(D_METHOD("get_point_connections", "id"), &AStar2D::get_point_connections);
+	ClassDB::bind_method(D_METHOD("get_point_ids"), &AStar2D::get_point_ids);
+
+	ClassDB::bind_method(D_METHOD("set_neighbor_filter_enabled", "enabled"), &AStar2D::set_neighbor_filter_enabled);
+	ClassDB::bind_method(D_METHOD("is_neighbor_filter_enabled"), &AStar2D::is_neighbor_filter_enabled);
+
+	ClassDB::bind_method(D_METHOD("set_point_disabled", "id", "disabled"), &AStar2D::set_point_disabled, DEFVAL(true));
+	ClassDB::bind_method(D_METHOD("is_point_disabled", "id"), &AStar2D::is_point_disabled);
+
+	ClassDB::bind_method(D_METHOD("connect_points", "id", "to_id", "bidirectional"), &AStar2D::connect_points, DEFVAL(true));
+	ClassDB::bind_method(D_METHOD("disconnect_points", "id", "to_id", "bidirectional"), &AStar2D::disconnect_points, DEFVAL(true));
+	ClassDB::bind_method(D_METHOD("are_points_connected", "id", "to_id", "bidirectional"), &AStar2D::are_points_connected, DEFVAL(true));
+
+	ClassDB::bind_method(D_METHOD("get_point_count"), &AStar2D::get_point_count);
+	ClassDB::bind_method(D_METHOD("get_point_capacity"), &AStar2D::get_point_capacity);
+	ClassDB::bind_method(D_METHOD("reserve_space", "num_nodes"), &AStar2D::reserve_space);
+	ClassDB::bind_method(D_METHOD("clear"), &AStar2D::clear);
+
+	ClassDB::bind_method(D_METHOD("get_closest_point", "to_position", "include_disabled"), &AStar2D::get_closest_point, DEFVAL(false));
+	ClassDB::bind_method(D_METHOD("get_closest_position_in_segment", "to_position"), &AStar2D::get_closest_position_in_segment);
+
+	ClassDB::bind_method(D_METHOD("get_point_path", "from_id", "to_id", "allow_partial_path"), &AStar2D::get_point_path, DEFVAL(false));
+	ClassDB::bind_method(D_METHOD("get_id_path", "from_id", "to_id", "allow_partial_path"), &AStar2D::get_id_path, DEFVAL(false));
+
+	GDVIRTUAL_BIND(_filter_neighbor, "from_id", "neighbor_id")
+	GDVIRTUAL_BIND(_estimate_cost, "from_id", "end_id")
+	GDVIRTUAL_BIND(_compute_cost, "from_id", "to_id")
+
+	ADD_PROPERTY(PropertyInfo(Variant::BOOL, "neighbor_filter_enabled"), "set_neighbor_filter_enabled", "is_neighbor_filter_enabled");
+}

core/math/a_star.h

@@ -0,0 +1,232 @@
+/**************************************************************************/
+/*  a_star.h                                                              */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "core/object/gdvirtual.gen.inc"
+#include "core/object/ref_counted.h"
+#include "core/templates/a_hash_map.h"
+
+/**
+	A* pathfinding algorithm.
+*/
+
+class AStar3D : public RefCounted {
+	GDCLASS(AStar3D, RefCounted);
+	friend class AStar2D;
+
+	struct Point {
+		Point() {}
+
+		int64_t id = 0;
+		Vector3 pos;
+		real_t weight_scale = 0;
+		bool enabled = false;
+
+		AHashMap<int64_t, Point *> neighbors = 4u;
+		AHashMap<int64_t, Point *> unlinked_neighbours = 4u;
+
+		// Used for pathfinding.
+		Point *prev_point = nullptr;
+		real_t g_score = 0;
+		real_t f_score = 0;
+		uint64_t open_pass = 0;
+		uint64_t closed_pass = 0;
+
+		// Used for getting closest_point_of_last_pathing_call.
+		real_t abs_g_score = 0;
+		real_t abs_f_score = 0;
+	};
+
+	struct SortPoints {
+		_FORCE_INLINE_ bool operator()(const Point *A, const Point *B) const { // Returns true when the Point A is worse than Point B.
+			if (A->f_score > B->f_score) {
+				return true;
+			} else if (A->f_score < B->f_score) {
+				return false;
+			} else {
+				return A->g_score < B->g_score; // If the f_costs are the same then prioritize the points that are further away from the start.
+			}
+		}
+	};
+
+	struct Segment {
+		Pair<int64_t, int64_t> key;
+
+		enum {
+			NONE = 0,
+			FORWARD = 1,
+			BACKWARD = 2,
+			BIDIRECTIONAL = FORWARD | BACKWARD
+		};
+		unsigned char direction = NONE;
+
+		static uint32_t hash(const Segment &p_seg) {
+			return HashMapHasherDefault::hash(p_seg.key);
+		}
+		bool operator==(const Segment &p_s) const { return key == p_s.key; }
+
+		Segment() {}
+		Segment(int64_t p_from, int64_t p_to) {
+			if (p_from < p_to) {
+				key.first = p_from;
+				key.second = p_to;
+				direction = FORWARD;
+			} else {
+				key.first = p_to;
+				key.second = p_from;
+				direction = BACKWARD;
+			}
+		}
+	};
+
+	mutable int64_t last_free_id = 0;
+	uint64_t pass = 1;
+
+	AHashMap<int64_t, Point *> points;
+	HashSet<Segment, Segment> segments;
+	Point *last_closest_point = nullptr;
+	bool neighbor_filter_enabled = false;
+
+	bool _solve(Point *begin_point, Point *end_point, bool p_allow_partial_path);
+
+protected:
+	static void _bind_methods();
+
+	virtual real_t _estimate_cost(int64_t p_from_id, int64_t p_end_id);
+	virtual real_t _compute_cost(int64_t p_from_id, int64_t p_to_id);
+
+	GDVIRTUAL2RC(bool, _filter_neighbor, int64_t, int64_t)
+	GDVIRTUAL2RC(real_t, _estimate_cost, int64_t, int64_t)
+	GDVIRTUAL2RC(real_t, _compute_cost, int64_t, int64_t)
+
+#ifndef DISABLE_DEPRECATED
+	Vector<int64_t> _get_id_path_bind_compat_88047(int64_t p_from_id, int64_t p_to_id);
+	Vector<Vector3> _get_point_path_bind_compat_88047(int64_t p_from_id, int64_t p_to_id);
+	static void _bind_compatibility_methods();
+#endif
+
+public:
+	int64_t get_available_point_id() const;
+
+	void add_point(int64_t p_id, const Vector3 &p_pos, real_t p_weight_scale = 1);
+	Vector3 get_point_position(int64_t p_id) const;
+	void set_point_position(int64_t p_id, const Vector3 &p_pos);
+	real_t get_point_weight_scale(int64_t p_id) const;
+	void set_point_weight_scale(int64_t p_id, real_t p_weight_scale);
+	void remove_point(int64_t p_id);
+	bool has_point(int64_t p_id) const;
+	Vector<int64_t> get_point_connections(int64_t p_id);
+	PackedInt64Array get_point_ids();
+
+	bool is_neighbor_filter_enabled() const;
+	void set_neighbor_filter_enabled(bool p_enabled);
+
+	void set_point_disabled(int64_t p_id, bool p_disabled = true);
+	bool is_point_disabled(int64_t p_id) const;
+
+	void connect_points(int64_t p_id, int64_t p_with_id, bool bidirectional = true);
+	void disconnect_points(int64_t p_id, int64_t p_with_id, bool bidirectional = true);
+	bool are_points_connected(int64_t p_id, int64_t p_with_id, bool bidirectional = true) const;
+
+	int64_t get_point_count() const;
+	int64_t get_point_capacity() const;
+	void reserve_space(int64_t p_num_nodes);
+	void clear();
+
+	int64_t get_closest_point(const Vector3 &p_point, bool p_include_disabled = false) const;
+	Vector3 get_closest_position_in_segment(const Vector3 &p_point) const;
+
+	Vector<Vector3> get_point_path(int64_t p_from_id, int64_t p_to_id, bool p_allow_partial_path = false);
+	Vector<int64_t> get_id_path(int64_t p_from_id, int64_t p_to_id, bool p_allow_partial_path = false);
+
+	AStar3D() {}
+	~AStar3D();
+};
+
+class AStar2D : public RefCounted {
+	GDCLASS(AStar2D, RefCounted);
+	AStar3D astar;
+
+	bool _solve(AStar3D::Point *begin_point, AStar3D::Point *end_point, bool p_allow_partial_path);
+
+protected:
+	static void _bind_methods();
+
+	virtual real_t _estimate_cost(int64_t p_from_id, int64_t p_end_id);
+	virtual real_t _compute_cost(int64_t p_from_id, int64_t p_to_id);
+
+	GDVIRTUAL2RC(bool, _filter_neighbor, int64_t, int64_t)
+	GDVIRTUAL2RC(real_t, _estimate_cost, int64_t, int64_t)
+	GDVIRTUAL2RC(real_t, _compute_cost, int64_t, int64_t)
+
+#ifndef DISABLE_DEPRECATED
+	Vector<int64_t> _get_id_path_bind_compat_88047(int64_t p_from_id, int64_t p_to_id);
+	Vector<Vector2> _get_point_path_bind_compat_88047(int64_t p_from_id, int64_t p_to_id);
+	static void _bind_compatibility_methods();
+#endif
+
+public:
+	int64_t get_available_point_id() const;
+
+	void add_point(int64_t p_id, const Vector2 &p_pos, real_t p_weight_scale = 1);
+	Vector2 get_point_position(int64_t p_id) const;
+	void set_point_position(int64_t p_id, const Vector2 &p_pos);
+	real_t get_point_weight_scale(int64_t p_id) const;
+	void set_point_weight_scale(int64_t p_id, real_t p_weight_scale);
+	void remove_point(int64_t p_id);
+	bool has_point(int64_t p_id) const;
+	Vector<int64_t> get_point_connections(int64_t p_id);
+	PackedInt64Array get_point_ids();
+
+	bool is_neighbor_filter_enabled() const;
+	void set_neighbor_filter_enabled(bool p_enabled);
+
+	void set_point_disabled(int64_t p_id, bool p_disabled = true);
+	bool is_point_disabled(int64_t p_id) const;
+
+	void connect_points(int64_t p_id, int64_t p_with_id, bool p_bidirectional = true);
+	void disconnect_points(int64_t p_id, int64_t p_with_id, bool p_bidirectional = true);
+	bool are_points_connected(int64_t p_id, int64_t p_with_id, bool p_bidirectional = true) const;
+
+	int64_t get_point_count() const;
+	int64_t get_point_capacity() const;
+	void reserve_space(int64_t p_num_nodes);
+	void clear();
+
+	int64_t get_closest_point(const Vector2 &p_point, bool p_include_disabled = false) const;
+	Vector2 get_closest_position_in_segment(const Vector2 &p_point) const;
+
+	Vector<Vector2> get_point_path(int64_t p_from_id, int64_t p_to_id, bool p_allow_partial_path = false);
+	Vector<int64_t> get_id_path(int64_t p_from_id, int64_t p_to_id, bool p_allow_partial_path = false);
+
+	AStar2D() {}
+	~AStar2D() {}
+};

core/math/a_star_grid_2d.compat.inc

@@ -0,0 +1,48 @@
+/**************************************************************************/
+/*  a_star_grid_2d.compat.inc                                             */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#ifndef DISABLE_DEPRECATED
+
+#include "core/variant/typed_array.h"
+
+TypedArray<Vector2i> AStarGrid2D::_get_id_path_bind_compat_88047(const Vector2i &p_from_id, const Vector2i &p_to_id) {
+	return get_id_path(p_from_id, p_to_id, false);
+}
+
+Vector<Vector2> AStarGrid2D::_get_point_path_bind_compat_88047(const Vector2i &p_from_id, const Vector2i &p_to_id) {
+	return get_point_path(p_from_id, p_to_id, false);
+}
+
+void AStarGrid2D::_bind_compatibility_methods() {
+	ClassDB::bind_compatibility_method(D_METHOD("get_id_path", "from_id", "to_id"), &AStarGrid2D::_get_id_path_bind_compat_88047);
+	ClassDB::bind_compatibility_method(D_METHOD("get_point_path", "from_id", "to_id"), &AStarGrid2D::_get_point_path_bind_compat_88047);
+}
+
+#endif // DISABLE_DEPRECATED

core/math/a_star_grid_2d.cpp

@@ -0,0 +1,804 @@
+/**************************************************************************/
+/*  a_star_grid_2d.cpp                                                    */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "a_star_grid_2d.h"
+#include "a_star_grid_2d.compat.inc"
+
+#include "core/variant/typed_array.h"
+
+static real_t heuristic_euclidean(const Vector2i &p_from, const Vector2i &p_to) {
+	real_t dx = (real_t)Math::abs(p_to.x - p_from.x);
+	real_t dy = (real_t)Math::abs(p_to.y - p_from.y);
+	return (real_t)Math::sqrt(dx * dx + dy * dy);
+}
+
+static real_t heuristic_manhattan(const Vector2i &p_from, const Vector2i &p_to) {
+	real_t dx = (real_t)Math::abs(p_to.x - p_from.x);
+	real_t dy = (real_t)Math::abs(p_to.y - p_from.y);
+	return dx + dy;
+}
+
+static real_t heuristic_octile(const Vector2i &p_from, const Vector2i &p_to) {
+	real_t dx = (real_t)Math::abs(p_to.x - p_from.x);
+	real_t dy = (real_t)Math::abs(p_to.y - p_from.y);
+	real_t F = Math::SQRT2 - 1;
+	return (dx < dy) ? F * dx + dy : F * dy + dx;
+}
+
+static real_t heuristic_chebyshev(const Vector2i &p_from, const Vector2i &p_to) {
+	real_t dx = (real_t)Math::abs(p_to.x - p_from.x);
+	real_t dy = (real_t)Math::abs(p_to.y - p_from.y);
+	return MAX(dx, dy);
+}
+
+static real_t (*heuristics[AStarGrid2D::HEURISTIC_MAX])(const Vector2i &, const Vector2i &) = { heuristic_euclidean, heuristic_manhattan, heuristic_octile, heuristic_chebyshev };
+
+void AStarGrid2D::set_region(const Rect2i &p_region) {
+	ERR_FAIL_COND(p_region.size.x < 0 || p_region.size.y < 0);
+	if (p_region != region) {
+		region = p_region;
+		dirty = true;
+	}
+}
+
+Rect2i AStarGrid2D::get_region() const {
+	return region;
+}
+
+void AStarGrid2D::set_size(const Size2i &p_size) {
+	WARN_DEPRECATED_MSG(R"(The "size" property is deprecated, use "region" instead.)");
+	ERR_FAIL_COND(p_size.x < 0 || p_size.y < 0);
+	if (p_size != region.size) {
+		region.size = p_size;
+		dirty = true;
+	}
+}
+
+Size2i AStarGrid2D::get_size() const {
+	return region.size;
+}
+
+void AStarGrid2D::set_offset(const Vector2 &p_offset) {
+	if (!offset.is_equal_approx(p_offset)) {
+		offset = p_offset;
+		dirty = true;
+	}
+}
+
+Vector2 AStarGrid2D::get_offset() const {
+	return offset;
+}
+
+void AStarGrid2D::set_cell_size(const Size2 &p_cell_size) {
+	if (!cell_size.is_equal_approx(p_cell_size)) {
+		cell_size = p_cell_size;
+		dirty = true;
+	}
+}
+
+Size2 AStarGrid2D::get_cell_size() const {
+	return cell_size;
+}
+
+void AStarGrid2D::set_cell_shape(CellShape p_cell_shape) {
+	if (cell_shape == p_cell_shape) {
+		return;
+	}
+
+	ERR_FAIL_INDEX(p_cell_shape, CellShape::CELL_SHAPE_MAX);
+	cell_shape = p_cell_shape;
+	dirty = true;
+}
+
+AStarGrid2D::CellShape AStarGrid2D::get_cell_shape() const {
+	return cell_shape;
+}
+
+void AStarGrid2D::update() {
+	if (!dirty) {
+		return;
+	}
+
+	points.clear();
+	solid_mask.clear();
+
+	const int32_t end_x = region.get_end().x;
+	const int32_t end_y = region.get_end().y;
+	const Vector2 half_cell_size = cell_size / 2;
+	for (int32_t x = region.position.x; x < end_x + 2; x++) {
+		solid_mask.push_back(true);
+	}
+
+	for (int32_t y = region.position.y; y < end_y; y++) {
+		LocalVector<Point> line;
+		solid_mask.push_back(true);
+		for (int32_t x = region.position.x; x < end_x; x++) {
+			Vector2 v = offset;
+			switch (cell_shape) {
+				case CELL_SHAPE_ISOMETRIC_RIGHT:
+					v += half_cell_size + Vector2(x + y, y - x) * half_cell_size;
+					break;
+				case CELL_SHAPE_ISOMETRIC_DOWN:
+					v += half_cell_size + Vector2(x - y, x + y) * half_cell_size;
+					break;
+				case CELL_SHAPE_SQUARE:
+					v += Vector2(x, y) * cell_size;
+					break;
+				default:
+					break;
+			}
+			line.push_back(Point(Vector2i(x, y), v));
+			solid_mask.push_back(false);
+		}
+		solid_mask.push_back(true);
+		points.push_back(line);
+	}
+
+	for (int32_t x = region.position.x; x < end_x + 2; x++) {
+		solid_mask.push_back(true);
+	}
+
+	dirty = false;
+}
+
+bool AStarGrid2D::is_in_bounds(int32_t p_x, int32_t p_y) const {
+	return region.has_point(Vector2i(p_x, p_y));
+}
+
+bool AStarGrid2D::is_in_boundsv(const Vector2i &p_id) const {
+	return region.has_point(p_id);
+}
+
+bool AStarGrid2D::is_dirty() const {
+	return dirty;
+}
+
+void AStarGrid2D::set_jumping_enabled(bool p_enabled) {
+	jumping_enabled = p_enabled;
+}
+
+bool AStarGrid2D::is_jumping_enabled() const {
+	return jumping_enabled;
+}
+
+void AStarGrid2D::set_diagonal_mode(DiagonalMode p_diagonal_mode) {
+	ERR_FAIL_INDEX((int)p_diagonal_mode, (int)DIAGONAL_MODE_MAX);
+	diagonal_mode = p_diagonal_mode;
+}
+
+AStarGrid2D::DiagonalMode AStarGrid2D::get_diagonal_mode() const {
+	return diagonal_mode;
+}
+
+void AStarGrid2D::set_default_compute_heuristic(Heuristic p_heuristic) {
+	ERR_FAIL_INDEX((int)p_heuristic, (int)HEURISTIC_MAX);
+	default_compute_heuristic = p_heuristic;
+}
+
+AStarGrid2D::Heuristic AStarGrid2D::get_default_compute_heuristic() const {
+	return default_compute_heuristic;
+}
+
+void AStarGrid2D::set_default_estimate_heuristic(Heuristic p_heuristic) {
+	ERR_FAIL_INDEX((int)p_heuristic, (int)HEURISTIC_MAX);
+	default_estimate_heuristic = p_heuristic;
+}
+
+AStarGrid2D::Heuristic AStarGrid2D::get_default_estimate_heuristic() const {
+	return default_estimate_heuristic;
+}
+
+void AStarGrid2D::set_point_solid(const Vector2i &p_id, bool p_solid) {
+	ERR_FAIL_COND_MSG(dirty, "Grid is not initialized. Call the update method.");
+	ERR_FAIL_COND_MSG(!is_in_boundsv(p_id), vformat("Can't set if point is disabled. Point %s out of bounds %s.", p_id, region));
+	_set_solid_unchecked(p_id, p_solid);
+}
+
+bool AStarGrid2D::is_point_solid(const Vector2i &p_id) const {
+	ERR_FAIL_COND_V_MSG(dirty, false, "Grid is not initialized. Call the update method.");
+	ERR_FAIL_COND_V_MSG(!is_in_boundsv(p_id), false, vformat("Can't get if point is disabled. Point %s out of bounds %s.", p_id, region));
+	return _get_solid_unchecked(p_id);
+}
+
+void AStarGrid2D::set_point_weight_scale(const Vector2i &p_id, real_t p_weight_scale) {
+	ERR_FAIL_COND_MSG(dirty, "Grid is not initialized. Call the update method.");
+	ERR_FAIL_COND_MSG(!is_in_boundsv(p_id), vformat("Can't set point's weight scale. Point %s out of bounds %s.", p_id, region));
+	ERR_FAIL_COND_MSG(p_weight_scale < 0.0, vformat("Can't set point's weight scale less than 0.0: %f.", p_weight_scale));
+	_get_point_unchecked(p_id)->weight_scale = p_weight_scale;
+}
+
+real_t AStarGrid2D::get_point_weight_scale(const Vector2i &p_id) const {
+	ERR_FAIL_COND_V_MSG(dirty, 0, "Grid is not initialized. Call the update method.");
+	ERR_FAIL_COND_V_MSG(!is_in_boundsv(p_id), 0, vformat("Can't get point's weight scale. Point %s out of bounds %s.", p_id, region));
+	return _get_point_unchecked(p_id)->weight_scale;
+}
+
+void AStarGrid2D::fill_solid_region(const Rect2i &p_region, bool p_solid) {
+	ERR_FAIL_COND_MSG(dirty, "Grid is not initialized. Call the update method.");
+
+	const Rect2i safe_region = p_region.intersection(region);
+	const int32_t end_x = safe_region.get_end().x;
+	const int32_t end_y = safe_region.get_end().y;
+
+	for (int32_t y = safe_region.position.y; y < end_y; y++) {
+		for (int32_t x = safe_region.position.x; x < end_x; x++) {
+			_set_solid_unchecked(x, y, p_solid);
+		}
+	}
+}
+
+void AStarGrid2D::fill_weight_scale_region(const Rect2i &p_region, real_t p_weight_scale) {
+	ERR_FAIL_COND_MSG(dirty, "Grid is not initialized. Call the update method.");
+	ERR_FAIL_COND_MSG(p_weight_scale < 0.0, vformat("Can't set point's weight scale less than 0.0: %f.", p_weight_scale));
+
+	const Rect2i safe_region = p_region.intersection(region);
+	const int32_t end_x = safe_region.get_end().x;
+	const int32_t end_y = safe_region.get_end().y;
+
+	for (int32_t y = safe_region.position.y; y < end_y; y++) {
+		for (int32_t x = safe_region.position.x; x < end_x; x++) {
+			_get_point_unchecked(x, y)->weight_scale = p_weight_scale;
+		}
+	}
+}
+
+AStarGrid2D::Point *AStarGrid2D::_jump(Point *p_from, Point *p_to) {
+	int32_t from_x = p_from->id.x;
+	int32_t from_y = p_from->id.y;
+
+	int32_t to_x = p_to->id.x;
+	int32_t to_y = p_to->id.y;
+
+	int32_t dx = to_x - from_x;
+	int32_t dy = to_y - from_y;
+
+	if (diagonal_mode == DIAGONAL_MODE_ALWAYS || diagonal_mode == DIAGONAL_MODE_AT_LEAST_ONE_WALKABLE) {
+		if (dx == 0 || dy == 0) {
+			return _forced_successor(to_x, to_y, dx, dy);
+		}
+
+		while (_is_walkable(to_x, to_y) && (diagonal_mode == DIAGONAL_MODE_ALWAYS || _is_walkable(to_x, to_y - dy) || _is_walkable(to_x - dx, to_y))) {
+			if (end->id.x == to_x && end->id.y == to_y) {
+				return end;
+			}
+
+			if ((_is_walkable(to_x - dx, to_y + dy) && !_is_walkable(to_x - dx, to_y)) || (_is_walkable(to_x + dx, to_y - dy) && !_is_walkable(to_x, to_y - dy))) {
+				return _get_point_unchecked(to_x, to_y);
+			}
+
+			if (_forced_successor(to_x + dx, to_y, dx, 0) != nullptr || _forced_successor(to_x, to_y + dy, 0, dy) != nullptr) {
+				return _get_point_unchecked(to_x, to_y);
+			}
+
+			to_x += dx;
+			to_y += dy;
+		}
+
+	} else if (diagonal_mode == DIAGONAL_MODE_ONLY_IF_NO_OBSTACLES) {
+		if (dx == 0 || dy == 0) {
+			return _forced_successor(from_x, from_y, dx, dy, true);
+		}
+
+		while (_is_walkable(to_x, to_y) && _is_walkable(to_x, to_y - dy) && _is_walkable(to_x - dx, to_y)) {
+			if (end->id.x == to_x && end->id.y == to_y) {
+				return end;
+			}
+
+			if ((_is_walkable(to_x + dx, to_y + dy) && !_is_walkable(to_x, to_y + dy)) || !_is_walkable(to_x + dx, to_y)) {
+				return _get_point_unchecked(to_x, to_y);
+			}
+
+			if (_forced_successor(to_x, to_y, dx, 0) != nullptr || _forced_successor(to_x, to_y, 0, dy) != nullptr) {
+				return _get_point_unchecked(to_x, to_y);
+			}
+
+			to_x += dx;
+			to_y += dy;
+		}
+
+	} else { // DIAGONAL_MODE_NEVER
+		if (dy == 0) {
+			return _forced_successor(from_x, from_y, dx, 0, true);
+		}
+
+		while (_is_walkable(to_x, to_y)) {
+			if (end->id.x == to_x && end->id.y == to_y) {
+				return end;
+			}
+
+			if ((_is_walkable(to_x - 1, to_y) && !_is_walkable(to_x - 1, to_y - dy)) || (_is_walkable(to_x + 1, to_y) && !_is_walkable(to_x + 1, to_y - dy))) {
+				return _get_point_unchecked(to_x, to_y);
+			}
+
+			if (_forced_successor(to_x, to_y, 1, 0, true) != nullptr || _forced_successor(to_x, to_y, -1, 0, true) != nullptr) {
+				return _get_point_unchecked(to_x, to_y);
+			}
+
+			to_y += dy;
+		}
+	}
+
+	return nullptr;
+}
+
+AStarGrid2D::Point *AStarGrid2D::_forced_successor(int32_t p_x, int32_t p_y, int32_t p_dx, int32_t p_dy, bool p_inclusive) {
+	// Remembering previous results can improve performance.
+	bool l_prev = false, r_prev = false, l = false, r = false;
+
+	int32_t o_x = p_x, o_y = p_y;
+	if (p_inclusive) {
+		o_x += p_dx;
+		o_y += p_dy;
+	}
+
+	int32_t l_x = p_x - p_dy, l_y = p_y - p_dx;
+	int32_t r_x = p_x + p_dy, r_y = p_y + p_dx;
+
+	while (_is_walkable(o_x, o_y)) {
+		if (end->id.x == o_x && end->id.y == o_y) {
+			return end;
+		}
+
+		l_prev = l || _is_walkable(l_x, l_y);
+		r_prev = r || _is_walkable(r_x, r_y);
+
+		l_x += p_dx;
+		l_y += p_dy;
+		r_x += p_dx;
+		r_y += p_dy;
+
+		l = _is_walkable(l_x, l_y);
+		r = _is_walkable(r_x, r_y);
+
+		if ((l && !l_prev) || (r && !r_prev)) {
+			return _get_point_unchecked(o_x, o_y);
+		}
+
+		o_x += p_dx;
+		o_y += p_dy;
+	}
+	return nullptr;
+}
+
+void AStarGrid2D::_get_nbors(Point *p_point, LocalVector<Point *> &r_nbors) {
+	bool ts0 = false, td0 = false,
+		 ts1 = false, td1 = false,
+		 ts2 = false, td2 = false,
+		 ts3 = false, td3 = false;
+
+	Point *left = nullptr;
+	Point *right = nullptr;
+	Point *top = nullptr;
+	Point *bottom = nullptr;
+
+	Point *top_left = nullptr;
+	Point *top_right = nullptr;
+	Point *bottom_left = nullptr;
+	Point *bottom_right = nullptr;
+
+	{
+		bool has_left = false;
+		bool has_right = false;
+
+		if (p_point->id.x - 1 >= region.position.x) {
+			left = _get_point_unchecked(p_point->id.x - 1, p_point->id.y);
+			has_left = true;
+		}
+		if (p_point->id.x + 1 < region.position.x + region.size.width) {
+			right = _get_point_unchecked(p_point->id.x + 1, p_point->id.y);
+			has_right = true;
+		}
+		if (p_point->id.y - 1 >= region.position.y) {
+			top = _get_point_unchecked(p_point->id.x, p_point->id.y - 1);
+			if (has_left) {
+				top_left = _get_point_unchecked(p_point->id.x - 1, p_point->id.y - 1);
+			}
+			if (has_right) {
+				top_right = _get_point_unchecked(p_point->id.x + 1, p_point->id.y - 1);
+			}
+		}
+		if (p_point->id.y + 1 < region.position.y + region.size.height) {
+			bottom = _get_point_unchecked(p_point->id.x, p_point->id.y + 1);
+			if (has_left) {
+				bottom_left = _get_point_unchecked(p_point->id.x - 1, p_point->id.y + 1);
+			}
+			if (has_right) {
+				bottom_right = _get_point_unchecked(p_point->id.x + 1, p_point->id.y + 1);
+			}
+		}
+	}
+
+	if (top && !_get_solid_unchecked(top->id)) {
+		r_nbors.push_back(top);
+		ts0 = true;
+	}
+	if (right && !_get_solid_unchecked(right->id)) {
+		r_nbors.push_back(right);
+		ts1 = true;
+	}
+	if (bottom && !_get_solid_unchecked(bottom->id)) {
+		r_nbors.push_back(bottom);
+		ts2 = true;
+	}
+	if (left && !_get_solid_unchecked(left->id)) {
+		r_nbors.push_back(left);
+		ts3 = true;
+	}
+
+	switch (diagonal_mode) {
+		case DIAGONAL_MODE_ALWAYS: {
+			td0 = true;
+			td1 = true;
+			td2 = true;
+			td3 = true;
+		} break;
+		case DIAGONAL_MODE_NEVER: {
+		} break;
+		case DIAGONAL_MODE_AT_LEAST_ONE_WALKABLE: {
+			td0 = ts3 || ts0;
+			td1 = ts0 || ts1;
+			td2 = ts1 || ts2;
+			td3 = ts2 || ts3;
+		} break;
+		case DIAGONAL_MODE_ONLY_IF_NO_OBSTACLES: {
+			td0 = ts3 && ts0;
+			td1 = ts0 && ts1;
+			td2 = ts1 && ts2;
+			td3 = ts2 && ts3;
+		} break;
+		default:
+			break;
+	}
+
+	if (td0 && (top_left && !_get_solid_unchecked(top_left->id))) {
+		r_nbors.push_back(top_left);
+	}
+	if (td1 && (top_right && !_get_solid_unchecked(top_right->id))) {
+		r_nbors.push_back(top_right);
+	}
+	if (td2 && (bottom_right && !_get_solid_unchecked(bottom_right->id))) {
+		r_nbors.push_back(bottom_right);
+	}
+	if (td3 && (bottom_left && !_get_solid_unchecked(bottom_left->id))) {
+		r_nbors.push_back(bottom_left);
+	}
+}
+
+bool AStarGrid2D::_solve(Point *p_begin_point, Point *p_end_point, bool p_allow_partial_path) {
+	last_closest_point = nullptr;
+	pass++;
+
+	if (_get_solid_unchecked(p_end_point->id) && !p_allow_partial_path) {
+		return false;
+	}
+
+	bool found_route = false;
+
+	LocalVector<Point *> open_list;
+	SortArray<Point *, SortPoints> sorter;
+	LocalVector<Point *> nbors;
+
+	p_begin_point->g_score = 0;
+	p_begin_point->f_score = _estimate_cost(p_begin_point->id, p_end_point->id);
+	p_begin_point->abs_g_score = 0;
+	p_begin_point->abs_f_score = _estimate_cost(p_begin_point->id, p_end_point->id);
+	open_list.push_back(p_begin_point);
+	end = p_end_point;
+
+	while (!open_list.is_empty()) {
+		Point *p = open_list[0]; // The currently processed point.
+
+		// Find point closer to end_point, or same distance to end_point but closer to begin_point.
+		if (last_closest_point == nullptr || last_closest_point->abs_f_score > p->abs_f_score || (last_closest_point->abs_f_score >= p->abs_f_score && last_closest_point->abs_g_score > p->abs_g_score)) {
+			last_closest_point = p;
+		}
+
+		if (p == p_end_point) {
+			found_route = true;
+			break;
+		}
+
+		sorter.pop_heap(0, open_list.size(), open_list.ptr()); // Remove the current point from the open list.
+		open_list.remove_at(open_list.size() - 1);
+		p->closed_pass = pass; // Mark the point as closed.
+
+		nbors.clear();
+		_get_nbors(p, nbors);
+
+		for (Point *e : nbors) {
+			real_t weight_scale = 1.0;
+
+			if (jumping_enabled) {
+				// TODO: Make it works with weight_scale.
+				e = _jump(p, e);
+				if (!e || e->closed_pass == pass) {
+					continue;
+				}
+			} else {
+				if (_get_solid_unchecked(e->id) || e->closed_pass == pass) {
+					continue;
+				}
+				weight_scale = e->weight_scale;
+			}
+
+			real_t tentative_g_score = p->g_score + _compute_cost(p->id, e->id) * weight_scale;
+			bool new_point = false;
+
+			if (e->open_pass != pass) { // The point wasn't inside the open list.
+				e->open_pass = pass;
+				open_list.push_back(e);
+				new_point = true;
+			} else if (tentative_g_score >= e->g_score) { // The new path is worse than the previous.
+				continue;
+			}
+
+			e->prev_point = p;
+			e->g_score = tentative_g_score;
+			e->f_score = e->g_score + _estimate_cost(e->id, p_end_point->id);
+
+			e->abs_g_score = tentative_g_score;
+			e->abs_f_score = e->f_score - e->g_score;
+
+			if (new_point) { // The position of the new points is already known.
+				sorter.push_heap(0, open_list.size() - 1, 0, e, open_list.ptr());
+			} else {
+				sorter.push_heap(0, open_list.find(e), 0, e, open_list.ptr());
+			}
+		}
+	}
+
+	return found_route;
+}
+
+real_t AStarGrid2D::_estimate_cost(const Vector2i &p_from_id, const Vector2i &p_end_id) {
+	real_t scost;
+	if (GDVIRTUAL_CALL(_estimate_cost, p_from_id, p_end_id, scost)) {
+		return scost;
+	}
+	return heuristics[default_estimate_heuristic](p_from_id, p_end_id);
+}
+
+real_t AStarGrid2D::_compute_cost(const Vector2i &p_from_id, const Vector2i &p_to_id) {
+	real_t scost;
+	if (GDVIRTUAL_CALL(_compute_cost, p_from_id, p_to_id, scost)) {
+		return scost;
+	}
+	return heuristics[default_compute_heuristic](p_from_id, p_to_id);
+}
+
+void AStarGrid2D::clear() {
+	points.clear();
+	region = Rect2i();
+}
+
+Vector2 AStarGrid2D::get_point_position(const Vector2i &p_id) const {
+	ERR_FAIL_COND_V_MSG(dirty, Vector2(), "Grid is not initialized. Call the update method.");
+	ERR_FAIL_COND_V_MSG(!is_in_boundsv(p_id), Vector2(), vformat("Can't get point's position. Point %s out of bounds %s.", p_id, region));
+	return _get_point_unchecked(p_id)->pos;
+}
+
+TypedArray<Dictionary> AStarGrid2D::get_point_data_in_region(const Rect2i &p_region) const {
+	ERR_FAIL_COND_V_MSG(dirty, TypedArray<Dictionary>(), "Grid is not initialized. Call the update method.");
+	const Rect2i inter_region = region.intersection(p_region);
+
+	const int32_t start_x = inter_region.position.x - region.position.x;
+	const int32_t start_y = inter_region.position.y - region.position.y;
+	const int32_t end_x = inter_region.get_end().x - region.position.x;
+	const int32_t end_y = inter_region.get_end().y - region.position.y;
+
+	TypedArray<Dictionary> data;
+
+	for (int32_t y = start_y; y < end_y; y++) {
+		for (int32_t x = start_x; x < end_x; x++) {
+			const Point &p = points[y][x];
+
+			Dictionary dict;
+			dict["id"] = p.id;
+			dict["position"] = p.pos;
+			dict["solid"] = _get_solid_unchecked(p.id);
+			dict["weight_scale"] = p.weight_scale;
+			data.push_back(dict);
+		}
+	}
+
+	return data;
+}
+
+Vector<Vector2> AStarGrid2D::get_point_path(const Vector2i &p_from_id, const Vector2i &p_to_id, bool p_allow_partial_path) {
+	ERR_FAIL_COND_V_MSG(dirty, Vector<Vector2>(), "Grid is not initialized. Call the update method.");
+	ERR_FAIL_COND_V_MSG(!is_in_boundsv(p_from_id), Vector<Vector2>(), vformat("Can't get id path. Point %s out of bounds %s.", p_from_id, region));
+	ERR_FAIL_COND_V_MSG(!is_in_boundsv(p_to_id), Vector<Vector2>(), vformat("Can't get id path. Point %s out of bounds %s.", p_to_id, region));
+
+	Point *a = _get_point(p_from_id.x, p_from_id.y);
+	Point *b = _get_point(p_to_id.x, p_to_id.y);
+
+	if (a == b) {
+		Vector<Vector2> ret;
+		ret.push_back(a->pos);
+		return ret;
+	}
+
+	Point *begin_point = a;
+	Point *end_point = b;
+
+	bool found_route = _solve(begin_point, end_point, p_allow_partial_path);
+	if (!found_route) {
+		if (!p_allow_partial_path || last_closest_point == nullptr) {
+			return Vector<Vector2>();
+		}
+
+		// Use closest point instead.
+		end_point = last_closest_point;
+	}
+
+	Point *p = end_point;
+	int32_t pc = 1;
+	while (p != begin_point) {
+		pc++;
+		p = p->prev_point;
+	}
+
+	Vector<Vector2> path;
+	path.resize(pc);
+
+	{
+		Vector2 *w = path.ptrw();
+
+		p = end_point;
+		int32_t idx = pc - 1;
+		while (p != begin_point) {
+			w[idx--] = p->pos;
+			p = p->prev_point;
+		}
+
+		w[0] = p->pos;
+	}
+
+	return path;
+}
+
+TypedArray<Vector2i> AStarGrid2D::get_id_path(const Vector2i &p_from_id, const Vector2i &p_to_id, bool p_allow_partial_path) {
+	ERR_FAIL_COND_V_MSG(dirty, TypedArray<Vector2i>(), "Grid is not initialized. Call the update method.");
+	ERR_FAIL_COND_V_MSG(!is_in_boundsv(p_from_id), TypedArray<Vector2i>(), vformat("Can't get id path. Point %s out of bounds %s.", p_from_id, region));
+	ERR_FAIL_COND_V_MSG(!is_in_boundsv(p_to_id), TypedArray<Vector2i>(), vformat("Can't get id path. Point %s out of bounds %s.", p_to_id, region));
+
+	Point *a = _get_point(p_from_id.x, p_from_id.y);
+	Point *b = _get_point(p_to_id.x, p_to_id.y);
+
+	if (a == b) {
+		TypedArray<Vector2i> ret;
+		ret.push_back(a->id);
+		return ret;
+	}
+
+	Point *begin_point = a;
+	Point *end_point = b;
+
+	bool found_route = _solve(begin_point, end_point, p_allow_partial_path);
+	if (!found_route) {
+		if (!p_allow_partial_path || last_closest_point == nullptr) {
+			return TypedArray<Vector2i>();
+		}
+
+		// Use closest point instead.
+		end_point = last_closest_point;
+	}
+
+	Point *p = end_point;
+	int32_t pc = 1;
+	while (p != begin_point) {
+		pc++;
+		p = p->prev_point;
+	}
+
+	TypedArray<Vector2i> path;
+	path.resize(pc);
+
+	{
+		p = end_point;
+		int32_t idx = pc - 1;
+		while (p != begin_point) {
+			path[idx--] = p->id;
+			p = p->prev_point;
+		}
+
+		path[0] = p->id;
+	}
+
+	return path;
+}
+
+void AStarGrid2D::_bind_methods() {
+	ClassDB::bind_method(D_METHOD("set_region", "region"), &AStarGrid2D::set_region);
+	ClassDB::bind_method(D_METHOD("get_region"), &AStarGrid2D::get_region);
+	ClassDB::bind_method(D_METHOD("set_size", "size"), &AStarGrid2D::set_size);
+	ClassDB::bind_method(D_METHOD("get_size"), &AStarGrid2D::get_size);
+	ClassDB::bind_method(D_METHOD("set_offset", "offset"), &AStarGrid2D::set_offset);
+	ClassDB::bind_method(D_METHOD("get_offset"), &AStarGrid2D::get_offset);
+	ClassDB::bind_method(D_METHOD("set_cell_size", "cell_size"), &AStarGrid2D::set_cell_size);
+	ClassDB::bind_method(D_METHOD("get_cell_size"), &AStarGrid2D::get_cell_size);
+	ClassDB::bind_method(D_METHOD("set_cell_shape", "cell_shape"), &AStarGrid2D::set_cell_shape);
+	ClassDB::bind_method(D_METHOD("get_cell_shape"), &AStarGrid2D::get_cell_shape);
+	ClassDB::bind_method(D_METHOD("is_in_bounds", "x", "y"), &AStarGrid2D::is_in_bounds);
+	ClassDB::bind_method(D_METHOD("is_in_boundsv", "id"), &AStarGrid2D::is_in_boundsv);
+	ClassDB::bind_method(D_METHOD("is_dirty"), &AStarGrid2D::is_dirty);
+	ClassDB::bind_method(D_METHOD("update"), &AStarGrid2D::update);
+	ClassDB::bind_method(D_METHOD("set_jumping_enabled", "enabled"), &AStarGrid2D::set_jumping_enabled);
+	ClassDB::bind_method(D_METHOD("is_jumping_enabled"), &AStarGrid2D::is_jumping_enabled);
+	ClassDB::bind_method(D_METHOD("set_diagonal_mode", "mode"), &AStarGrid2D::set_diagonal_mode);
+	ClassDB::bind_method(D_METHOD("get_diagonal_mode"), &AStarGrid2D::get_diagonal_mode);
+	ClassDB::bind_method(D_METHOD("set_default_compute_heuristic", "heuristic"), &AStarGrid2D::set_default_compute_heuristic);
+	ClassDB::bind_method(D_METHOD("get_default_compute_heuristic"), &AStarGrid2D::get_default_compute_heuristic);
+	ClassDB::bind_method(D_METHOD("set_default_estimate_heuristic", "heuristic"), &AStarGrid2D::set_default_estimate_heuristic);
+	ClassDB::bind_method(D_METHOD("get_default_estimate_heuristic"), &AStarGrid2D::get_default_estimate_heuristic);
+	ClassDB::bind_method(D_METHOD("set_point_solid", "id", "solid"), &AStarGrid2D::set_point_solid, DEFVAL(true));
+	ClassDB::bind_method(D_METHOD("is_point_solid", "id"), &AStarGrid2D::is_point_solid);
+	ClassDB::bind_method(D_METHOD("set_point_weight_scale", "id", "weight_scale"), &AStarGrid2D::set_point_weight_scale);
+	ClassDB::bind_method(D_METHOD("get_point_weight_scale", "id"), &AStarGrid2D::get_point_weight_scale);
+	ClassDB::bind_method(D_METHOD("fill_solid_region", "region", "solid"), &AStarGrid2D::fill_solid_region, DEFVAL(true));
+	ClassDB::bind_method(D_METHOD("fill_weight_scale_region", "region", "weight_scale"), &AStarGrid2D::fill_weight_scale_region);
+	ClassDB::bind_method(D_METHOD("clear"), &AStarGrid2D::clear);
+
+	ClassDB::bind_method(D_METHOD("get_point_position", "id"), &AStarGrid2D::get_point_position);
+	ClassDB::bind_method(D_METHOD("get_point_data_in_region", "region"), &AStarGrid2D::get_point_data_in_region);
+	ClassDB::bind_method(D_METHOD("get_point_path", "from_id", "to_id", "allow_partial_path"), &AStarGrid2D::get_point_path, DEFVAL(false));
+	ClassDB::bind_method(D_METHOD("get_id_path", "from_id", "to_id", "allow_partial_path"), &AStarGrid2D::get_id_path, DEFVAL(false));
+
+	GDVIRTUAL_BIND(_estimate_cost, "from_id", "end_id")
+	GDVIRTUAL_BIND(_compute_cost, "from_id", "to_id")
+
+	ADD_PROPERTY(PropertyInfo(Variant::RECT2I, "region"), "set_region", "get_region");
+	ADD_PROPERTY(PropertyInfo(Variant::VECTOR2I, "size"), "set_size", "get_size");
+	ADD_PROPERTY(PropertyInfo(Variant::VECTOR2, "offset"), "set_offset", "get_offset");
+	ADD_PROPERTY(PropertyInfo(Variant::VECTOR2, "cell_size"), "set_cell_size", "get_cell_size");
+	ADD_PROPERTY(PropertyInfo(Variant::INT, "cell_shape", PROPERTY_HINT_ENUM, "Square,IsometricRight,IsometricDown"), "set_cell_shape", "get_cell_shape");
+
+	ADD_PROPERTY(PropertyInfo(Variant::BOOL, "jumping_enabled"), "set_jumping_enabled", "is_jumping_enabled");
+	ADD_PROPERTY(PropertyInfo(Variant::INT, "default_compute_heuristic", PROPERTY_HINT_ENUM, "Euclidean,Manhattan,Octile,Chebyshev"), "set_default_compute_heuristic", "get_default_compute_heuristic");
+	ADD_PROPERTY(PropertyInfo(Variant::INT, "default_estimate_heuristic", PROPERTY_HINT_ENUM, "Euclidean,Manhattan,Octile,Chebyshev"), "set_default_estimate_heuristic", "get_default_estimate_heuristic");
+	ADD_PROPERTY(PropertyInfo(Variant::INT, "diagonal_mode", PROPERTY_HINT_ENUM, "Never,Always,At Least One Walkable,Only If No Obstacles"), "set_diagonal_mode", "get_diagonal_mode");
+
+	BIND_ENUM_CONSTANT(HEURISTIC_EUCLIDEAN);
+	BIND_ENUM_CONSTANT(HEURISTIC_MANHATTAN);
+	BIND_ENUM_CONSTANT(HEURISTIC_OCTILE);
+	BIND_ENUM_CONSTANT(HEURISTIC_CHEBYSHEV);
+	BIND_ENUM_CONSTANT(HEURISTIC_MAX);
+
+	BIND_ENUM_CONSTANT(DIAGONAL_MODE_ALWAYS);
+	BIND_ENUM_CONSTANT(DIAGONAL_MODE_NEVER);
+	BIND_ENUM_CONSTANT(DIAGONAL_MODE_AT_LEAST_ONE_WALKABLE);
+	BIND_ENUM_CONSTANT(DIAGONAL_MODE_ONLY_IF_NO_OBSTACLES);
+	BIND_ENUM_CONSTANT(DIAGONAL_MODE_MAX);
+
+	BIND_ENUM_CONSTANT(CELL_SHAPE_SQUARE);
+	BIND_ENUM_CONSTANT(CELL_SHAPE_ISOMETRIC_RIGHT);
+	BIND_ENUM_CONSTANT(CELL_SHAPE_ISOMETRIC_DOWN);
+	BIND_ENUM_CONSTANT(CELL_SHAPE_MAX);
+}

core/math/a_star_grid_2d.h

@@ -0,0 +1,231 @@
+/**************************************************************************/
+/*  a_star_grid_2d.h                                                      */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "core/object/gdvirtual.gen.inc"
+#include "core/object/ref_counted.h"
+#include "core/templates/local_vector.h"
+
+class AStarGrid2D : public RefCounted {
+	GDCLASS(AStarGrid2D, RefCounted);
+
+public:
+	enum DiagonalMode {
+		DIAGONAL_MODE_ALWAYS,
+		DIAGONAL_MODE_NEVER,
+		DIAGONAL_MODE_AT_LEAST_ONE_WALKABLE,
+		DIAGONAL_MODE_ONLY_IF_NO_OBSTACLES,
+		DIAGONAL_MODE_MAX,
+	};
+
+	enum Heuristic {
+		HEURISTIC_EUCLIDEAN,
+		HEURISTIC_MANHATTAN,
+		HEURISTIC_OCTILE,
+		HEURISTIC_CHEBYSHEV,
+		HEURISTIC_MAX,
+	};
+
+	enum CellShape {
+		CELL_SHAPE_SQUARE,
+		CELL_SHAPE_ISOMETRIC_RIGHT,
+		CELL_SHAPE_ISOMETRIC_DOWN,
+		CELL_SHAPE_MAX,
+	};
+
+private:
+	Rect2i region;
+	Vector2 offset;
+	Size2 cell_size = Size2(1, 1);
+	bool dirty = false;
+	CellShape cell_shape = CELL_SHAPE_SQUARE;
+
+	bool jumping_enabled = false;
+	DiagonalMode diagonal_mode = DIAGONAL_MODE_ALWAYS;
+	Heuristic default_compute_heuristic = HEURISTIC_EUCLIDEAN;
+	Heuristic default_estimate_heuristic = HEURISTIC_EUCLIDEAN;
+
+	struct Point {
+		Vector2i id;
+
+		Vector2 pos;
+		real_t weight_scale = 1.0;
+
+		// Used for pathfinding.
+		Point *prev_point = nullptr;
+		real_t g_score = 0;
+		real_t f_score = 0;
+		uint64_t open_pass = 0;
+		uint64_t closed_pass = 0;
+
+		// Used for getting last_closest_point.
+		real_t abs_g_score = 0;
+		real_t abs_f_score = 0;
+
+		Point() {}
+
+		Point(const Vector2i &p_id, const Vector2 &p_pos) :
+				id(p_id), pos(p_pos) {}
+	};
+
+	struct SortPoints {
+		_FORCE_INLINE_ bool operator()(const Point *A, const Point *B) const { // Returns true when the Point A is worse than Point B.
+			if (A->f_score > B->f_score) {
+				return true;
+			} else if (A->f_score < B->f_score) {
+				return false;
+			} else {
+				return A->g_score < B->g_score; // If the f_costs are the same then prioritize the points that are further away from the start.
+			}
+		}
+	};
+
+	LocalVector<bool> solid_mask;
+	LocalVector<LocalVector<Point>> points;
+	Point *end = nullptr;
+	Point *last_closest_point = nullptr;
+
+	uint64_t pass = 1;
+
+private: // Internal routines.
+	_FORCE_INLINE_ size_t _to_mask_index(int32_t p_x, int32_t p_y) const {
+		return ((p_y - region.position.y + 1) * (region.size.x + 2)) + p_x - region.position.x + 1;
+	}
+
+	_FORCE_INLINE_ bool _is_walkable(int32_t p_x, int32_t p_y) const {
+		return !solid_mask[_to_mask_index(p_x, p_y)];
+	}
+
+	_FORCE_INLINE_ Point *_get_point(int32_t p_x, int32_t p_y) {
+		if (region.has_point(Vector2i(p_x, p_y))) {
+			return &points[p_y - region.position.y][p_x - region.position.x];
+		}
+		return nullptr;
+	}
+
+	_FORCE_INLINE_ void _set_solid_unchecked(int32_t p_x, int32_t p_y, bool p_solid) {
+		solid_mask[_to_mask_index(p_x, p_y)] = p_solid;
+	}
+
+	_FORCE_INLINE_ void _set_solid_unchecked(const Vector2i &p_id, bool p_solid) {
+		solid_mask[_to_mask_index(p_id.x, p_id.y)] = p_solid;
+	}
+
+	_FORCE_INLINE_ bool _get_solid_unchecked(const Vector2i &p_id) const {
+		return solid_mask[_to_mask_index(p_id.x, p_id.y)];
+	}
+
+	_FORCE_INLINE_ Point *_get_point_unchecked(int32_t p_x, int32_t p_y) {
+		return &points[p_y - region.position.y][p_x - region.position.x];
+	}
+
+	_FORCE_INLINE_ Point *_get_point_unchecked(const Vector2i &p_id) {
+		return &points[p_id.y - region.position.y][p_id.x - region.position.x];
+	}
+
+	_FORCE_INLINE_ const Point *_get_point_unchecked(const Vector2i &p_id) const {
+		return &points[p_id.y - region.position.y][p_id.x - region.position.x];
+	}
+
+	void _get_nbors(Point *p_point, LocalVector<Point *> &r_nbors);
+	Point *_jump(Point *p_from, Point *p_to);
+	bool _solve(Point *p_begin_point, Point *p_end_point, bool p_allow_partial_path);
+	Point *_forced_successor(int32_t p_x, int32_t p_y, int32_t p_dx, int32_t p_dy, bool p_inclusive = false);
+
+protected:
+	static void _bind_methods();
+
+	virtual real_t _estimate_cost(const Vector2i &p_from_id, const Vector2i &p_end_id);
+	virtual real_t _compute_cost(const Vector2i &p_from_id, const Vector2i &p_to_id);
+
+	GDVIRTUAL2RC(real_t, _estimate_cost, Vector2i, Vector2i)
+	GDVIRTUAL2RC(real_t, _compute_cost, Vector2i, Vector2i)
+
+#ifndef DISABLE_DEPRECATED
+	TypedArray<Vector2i> _get_id_path_bind_compat_88047(const Vector2i &p_from, const Vector2i &p_to);
+	Vector<Vector2> _get_point_path_bind_compat_88047(const Vector2i &p_from, const Vector2i &p_to);
+	static void _bind_compatibility_methods();
+#endif
+
+public:
+	void set_region(const Rect2i &p_region);
+	Rect2i get_region() const;
+
+	void set_size(const Size2i &p_size);
+	Size2i get_size() const;
+
+	void set_offset(const Vector2 &p_offset);
+	Vector2 get_offset() const;
+
+	void set_cell_size(const Size2 &p_cell_size);
+	Size2 get_cell_size() const;
+
+	void set_cell_shape(CellShape p_cell_shape);
+	CellShape get_cell_shape() const;
+
+	void update();
+
+	bool is_in_bounds(int32_t p_x, int32_t p_y) const;
+	bool is_in_boundsv(const Vector2i &p_id) const;
+	bool is_dirty() const;
+
+	void set_jumping_enabled(bool p_enabled);
+	bool is_jumping_enabled() const;
+
+	void set_diagonal_mode(DiagonalMode p_diagonal_mode);
+	DiagonalMode get_diagonal_mode() const;
+
+	void set_default_compute_heuristic(Heuristic p_heuristic);
+	Heuristic get_default_compute_heuristic() const;
+
+	void set_default_estimate_heuristic(Heuristic p_heuristic);
+	Heuristic get_default_estimate_heuristic() const;
+
+	void set_point_solid(const Vector2i &p_id, bool p_solid = true);
+	bool is_point_solid(const Vector2i &p_id) const;
+
+	void set_point_weight_scale(const Vector2i &p_id, real_t p_weight_scale);
+	real_t get_point_weight_scale(const Vector2i &p_id) const;
+
+	void fill_solid_region(const Rect2i &p_region, bool p_solid = true);
+	void fill_weight_scale_region(const Rect2i &p_region, real_t p_weight_scale);
+
+	void clear();
+
+	Vector2 get_point_position(const Vector2i &p_id) const;
+	TypedArray<Dictionary> get_point_data_in_region(const Rect2i &p_region) const;
+	Vector<Vector2> get_point_path(const Vector2i &p_from, const Vector2i &p_to, bool p_allow_partial_path = false);
+	TypedArray<Vector2i> get_id_path(const Vector2i &p_from, const Vector2i &p_to, bool p_allow_partial_path = false);
+};
+
+VARIANT_ENUM_CAST(AStarGrid2D::DiagonalMode);
+VARIANT_ENUM_CAST(AStarGrid2D::Heuristic);
+VARIANT_ENUM_CAST(AStarGrid2D::CellShape)

core/math/aabb.cpp

@@ -0,0 +1,445 @@
+/**************************************************************************/
+/*  aabb.cpp                                                              */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "aabb.h"
+
+#include "core/string/ustring.h"
+#include "core/variant/variant.h"
+
+real_t AABB::get_volume() const {
+	return size.x * size.y * size.z;
+}
+
+void AABB::merge_with(const AABB &p_aabb) {
+#ifdef MATH_CHECKS
+	if (unlikely(size.x < 0 || size.y < 0 || size.z < 0 || p_aabb.size.x < 0 || p_aabb.size.y < 0 || p_aabb.size.z < 0)) {
+		ERR_PRINT("AABB size is negative, this is not supported. Use AABB.abs() to get an AABB with a positive size.");
+	}
+#endif
+	Vector3 beg_1, beg_2;
+	Vector3 end_1, end_2;
+	Vector3 min, max;
+
+	beg_1 = position;
+	beg_2 = p_aabb.position;
+	end_1 = size + beg_1;
+	end_2 = p_aabb.size + beg_2;
+
+	min.x = (beg_1.x < beg_2.x) ? beg_1.x : beg_2.x;
+	min.y = (beg_1.y < beg_2.y) ? beg_1.y : beg_2.y;
+	min.z = (beg_1.z < beg_2.z) ? beg_1.z : beg_2.z;
+
+	max.x = (end_1.x > end_2.x) ? end_1.x : end_2.x;
+	max.y = (end_1.y > end_2.y) ? end_1.y : end_2.y;
+	max.z = (end_1.z > end_2.z) ? end_1.z : end_2.z;
+
+	position = min;
+	size = max - min;
+}
+
+bool AABB::is_equal_approx(const AABB &p_aabb) const {
+	return position.is_equal_approx(p_aabb.position) && size.is_equal_approx(p_aabb.size);
+}
+
+bool AABB::is_same(const AABB &p_aabb) const {
+	return position.is_same(p_aabb.position) && size.is_same(p_aabb.size);
+}
+
+bool AABB::is_finite() const {
+	return position.is_finite() && size.is_finite();
+}
+
+AABB AABB::intersection(const AABB &p_aabb) const {
+#ifdef MATH_CHECKS
+	if (unlikely(size.x < 0 || size.y < 0 || size.z < 0 || p_aabb.size.x < 0 || p_aabb.size.y < 0 || p_aabb.size.z < 0)) {
+		ERR_PRINT("AABB size is negative, this is not supported. Use AABB.abs() to get an AABB with a positive size.");
+	}
+#endif
+	Vector3 src_min = position;
+	Vector3 src_max = position + size;
+	Vector3 dst_min = p_aabb.position;
+	Vector3 dst_max = p_aabb.position + p_aabb.size;
+
+	Vector3 min, max;
+
+	if (src_min.x > dst_max.x || src_max.x < dst_min.x) {
+		return AABB();
+	} else {
+		min.x = (src_min.x > dst_min.x) ? src_min.x : dst_min.x;
+		max.x = (src_max.x < dst_max.x) ? src_max.x : dst_max.x;
+	}
+
+	if (src_min.y > dst_max.y || src_max.y < dst_min.y) {
+		return AABB();
+	} else {
+		min.y = (src_min.y > dst_min.y) ? src_min.y : dst_min.y;
+		max.y = (src_max.y < dst_max.y) ? src_max.y : dst_max.y;
+	}
+
+	if (src_min.z > dst_max.z || src_max.z < dst_min.z) {
+		return AABB();
+	} else {
+		min.z = (src_min.z > dst_min.z) ? src_min.z : dst_min.z;
+		max.z = (src_max.z < dst_max.z) ? src_max.z : dst_max.z;
+	}
+
+	return AABB(min, max - min);
+}
+
+// Note that this routine returns the BACKTRACKED (i.e. behind the ray origin)
+// intersection point + normal if INSIDE the AABB.
+// The caller can therefore decide when INSIDE whether to use the
+// backtracked intersection, or use p_from as the intersection, and
+// carry on progressing without e.g. reflecting against the normal.
+bool AABB::find_intersects_ray(const Vector3 &p_from, const Vector3 &p_dir, bool &r_inside, Vector3 *r_intersection_point, Vector3 *r_normal) const {
+#ifdef MATH_CHECKS
+	if (unlikely(size.x < 0 || size.y < 0 || size.z < 0)) {
+		ERR_PRINT("AABB size is negative, this is not supported. Use AABB.abs() to get an AABB with a positive size.");
+	}
+#endif
+	Vector3 end = position + size;
+	real_t tmin = -1e20;
+	real_t tmax = 1e20;
+	int axis = 0;
+
+	// Make sure r_inside is always initialized,
+	// to prevent reading uninitialized data in the client code.
+	r_inside = false;
+
+	for (int i = 0; i < 3; i++) {
+		if (p_dir[i] == 0) {
+			if ((p_from[i] < position[i]) || (p_from[i] > end[i])) {
+				return false;
+			}
+		} else { // ray not parallel to planes in this direction
+			real_t t1 = (position[i] - p_from[i]) / p_dir[i];
+			real_t t2 = (end[i] - p_from[i]) / p_dir[i];
+
+			if (t1 > t2) {
+				SWAP(t1, t2);
+			}
+			if (t1 >= tmin) {
+				tmin = t1;
+				axis = i;
+			}
+			if (t2 < tmax) {
+				if (t2 < 0) {
+					return false;
+				}
+				tmax = t2;
+			}
+			if (tmin > tmax) {
+				return false;
+			}
+		}
+	}
+
+	// Did the ray start from inside the box?
+	// In which case the intersection returned is the point of entry
+	// (behind the ray start) or the calling routine can use the ray origin as intersection point.
+	r_inside = tmin < 0;
+
+	if (r_intersection_point) {
+		*r_intersection_point = p_from + p_dir * tmin;
+
+		// Prevent float error by making sure the point is exactly
+		// on the AABB border on the relevant axis.
+		r_intersection_point->coord[axis] = (p_dir[axis] >= 0) ? position.coord[axis] : end.coord[axis];
+	}
+	if (r_normal) {
+		*r_normal = Vector3();
+		(*r_normal)[axis] = (p_dir[axis] >= 0) ? -1 : 1;
+	}
+
+	return true;
+}
+
+bool AABB::intersects_segment(const Vector3 &p_from, const Vector3 &p_to, Vector3 *r_intersection_point, Vector3 *r_normal) const {
+#ifdef MATH_CHECKS
+	if (unlikely(size.x < 0 || size.y < 0 || size.z < 0)) {
+		ERR_PRINT("AABB size is negative, this is not supported. Use AABB.abs() to get an AABB with a positive size.");
+	}
+#endif
+	real_t min = 0, max = 1;
+	int axis = 0;
+	real_t sign = 0;
+
+	for (int i = 0; i < 3; i++) {
+		real_t seg_from = p_from[i];
+		real_t seg_to = p_to[i];
+		real_t box_begin = position[i];
+		real_t box_end = box_begin + size[i];
+		real_t cmin, cmax;
+		real_t csign;
+
+		if (seg_from < seg_to) {
+			if (seg_from > box_end || seg_to < box_begin) {
+				return false;
+			}
+			real_t length = seg_to - seg_from;
+			cmin = (seg_from < box_begin) ? ((box_begin - seg_from) / length) : 0;
+			cmax = (seg_to > box_end) ? ((box_end - seg_from) / length) : 1;
+			csign = -1.0;
+
+		} else {
+			if (seg_to > box_end || seg_from < box_begin) {
+				return false;
+			}
+			real_t length = seg_to - seg_from;
+			cmin = (seg_from > box_end) ? (box_end - seg_from) / length : 0;
+			cmax = (seg_to < box_begin) ? (box_begin - seg_from) / length : 1;
+			csign = 1.0;
+		}
+
+		if (cmin > min) {
+			min = cmin;
+			axis = i;
+			sign = csign;
+		}
+		if (cmax < max) {
+			max = cmax;
+		}
+		if (max < min) {
+			return false;
+		}
+	}
+
+	Vector3 rel = p_to - p_from;
+
+	if (r_normal) {
+		Vector3 normal;
+		normal[axis] = sign;
+		*r_normal = normal;
+	}
+
+	if (r_intersection_point) {
+		*r_intersection_point = p_from + rel * min;
+	}
+
+	return true;
+}
+
+bool AABB::intersects_plane(const Plane &p_plane) const {
+	Vector3 points[8] = {
+		Vector3(position.x, position.y, position.z),
+		Vector3(position.x, position.y, position.z + size.z),
+		Vector3(position.x, position.y + size.y, position.z),
+		Vector3(position.x, position.y + size.y, position.z + size.z),
+		Vector3(position.x + size.x, position.y, position.z),
+		Vector3(position.x + size.x, position.y, position.z + size.z),
+		Vector3(position.x + size.x, position.y + size.y, position.z),
+		Vector3(position.x + size.x, position.y + size.y, position.z + size.z),
+	};
+
+	bool over = false;
+	bool under = false;
+
+	for (int i = 0; i < 8; i++) {
+		if (p_plane.distance_to(points[i]) > 0) {
+			over = true;
+		} else {
+			under = true;
+		}
+	}
+
+	return under && over;
+}
+
+Vector3 AABB::get_longest_axis() const {
+	Vector3 axis(1, 0, 0);
+	real_t max_size = size.x;
+
+	if (size.y > max_size) {
+		axis = Vector3(0, 1, 0);
+		max_size = size.y;
+	}
+
+	if (size.z > max_size) {
+		axis = Vector3(0, 0, 1);
+	}
+
+	return axis;
+}
+
+int AABB::get_longest_axis_index() const {
+	int axis = 0;
+	real_t max_size = size.x;
+
+	if (size.y > max_size) {
+		axis = 1;
+		max_size = size.y;
+	}
+
+	if (size.z > max_size) {
+		axis = 2;
+	}
+
+	return axis;
+}
+
+Vector3 AABB::get_shortest_axis() const {
+	Vector3 axis(1, 0, 0);
+	real_t min_size = size.x;
+
+	if (size.y < min_size) {
+		axis = Vector3(0, 1, 0);
+		min_size = size.y;
+	}
+
+	if (size.z < min_size) {
+		axis = Vector3(0, 0, 1);
+	}
+
+	return axis;
+}
+
+int AABB::get_shortest_axis_index() const {
+	int axis = 0;
+	real_t min_size = size.x;
+
+	if (size.y < min_size) {
+		axis = 1;
+		min_size = size.y;
+	}
+
+	if (size.z < min_size) {
+		axis = 2;
+	}
+
+	return axis;
+}
+
+AABB AABB::merge(const AABB &p_with) const {
+	AABB aabb = *this;
+	aabb.merge_with(p_with);
+	return aabb;
+}
+
+AABB AABB::expand(const Vector3 &p_vector) const {
+	AABB aabb = *this;
+	aabb.expand_to(p_vector);
+	return aabb;
+}
+
+AABB AABB::grow(real_t p_by) const {
+	AABB aabb = *this;
+	aabb.grow_by(p_by);
+	return aabb;
+}
+
+void AABB::get_edge(int p_edge, Vector3 &r_from, Vector3 &r_to) const {
+	ERR_FAIL_INDEX(p_edge, 12);
+	switch (p_edge) {
+		case 0: {
+			r_from = Vector3(position.x + size.x, position.y, position.z);
+			r_to = Vector3(position.x, position.y, position.z);
+		} break;
+		case 1: {
+			r_from = Vector3(position.x + size.x, position.y, position.z + size.z);
+			r_to = Vector3(position.x + size.x, position.y, position.z);
+		} break;
+		case 2: {
+			r_from = Vector3(position.x, position.y, position.z + size.z);
+			r_to = Vector3(position.x + size.x, position.y, position.z + size.z);
+
+		} break;
+		case 3: {
+			r_from = Vector3(position.x, position.y, position.z);
+			r_to = Vector3(position.x, position.y, position.z + size.z);
+
+		} break;
+		case 4: {
+			r_from = Vector3(position.x, position.y + size.y, position.z);
+			r_to = Vector3(position.x + size.x, position.y + size.y, position.z);
+		} break;
+		case 5: {
+			r_from = Vector3(position.x + size.x, position.y + size.y, position.z);
+			r_to = Vector3(position.x + size.x, position.y + size.y, position.z + size.z);
+		} break;
+		case 6: {
+			r_from = Vector3(position.x + size.x, position.y + size.y, position.z + size.z);
+			r_to = Vector3(position.x, position.y + size.y, position.z + size.z);
+
+		} break;
+		case 7: {
+			r_from = Vector3(position.x, position.y + size.y, position.z + size.z);
+			r_to = Vector3(position.x, position.y + size.y, position.z);
+
+		} break;
+		case 8: {
+			r_from = Vector3(position.x, position.y, position.z + size.z);
+			r_to = Vector3(position.x, position.y + size.y, position.z + size.z);
+
+		} break;
+		case 9: {
+			r_from = Vector3(position.x, position.y, position.z);
+			r_to = Vector3(position.x, position.y + size.y, position.z);
+
+		} break;
+		case 10: {
+			r_from = Vector3(position.x + size.x, position.y, position.z);
+			r_to = Vector3(position.x + size.x, position.y + size.y, position.z);
+
+		} break;
+		case 11: {
+			r_from = Vector3(position.x + size.x, position.y, position.z + size.z);
+			r_to = Vector3(position.x + size.x, position.y + size.y, position.z + size.z);
+
+		} break;
+	}
+}
+
+Variant AABB::intersects_segment_bind(const Vector3 &p_from, const Vector3 &p_to) const {
+	Vector3 inters;
+	if (intersects_segment(p_from, p_to, &inters)) {
+		return inters;
+	}
+	return Variant();
+}
+
+Variant AABB::intersects_ray_bind(const Vector3 &p_from, const Vector3 &p_dir) const {
+	Vector3 inters;
+	bool inside = false;
+
+	if (find_intersects_ray(p_from, p_dir, inside, &inters)) {
+		// When inside the intersection point may be BEHIND the ray,
+		// so for general use we return the ray origin.
+		if (inside) {
+			return p_from;
+		}
+
+		return inters;
+	}
+	return Variant();
+}
+
+AABB::operator String() const {
+	return "[P: " + String(position) + ", S: " + String(size) + "]";
+}

core/math/aabb.h

@@ -0,0 +1,504 @@
+/**************************************************************************/
+/*  aabb.h                                                                */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "core/math/plane.h"
+#include "core/math/vector3.h"
+
+/**
+ * AABB (Axis Aligned Bounding Box)
+ * This is implemented by a point (position) and the box size.
+ */
+
+class Variant;
+
+struct [[nodiscard]] AABB {
+	Vector3 position;
+	Vector3 size;
+
+	real_t get_volume() const;
+	_FORCE_INLINE_ bool has_volume() const {
+		return size.x > 0.0f && size.y > 0.0f && size.z > 0.0f;
+	}
+
+	_FORCE_INLINE_ bool has_surface() const {
+		return size.x > 0.0f || size.y > 0.0f || size.z > 0.0f;
+	}
+
+	const Vector3 &get_position() const { return position; }
+	void set_position(const Vector3 &p_pos) { position = p_pos; }
+	const Vector3 &get_size() const { return size; }
+	void set_size(const Vector3 &p_size) { size = p_size; }
+
+	constexpr bool operator==(const AABB &p_rval) const {
+		return position == p_rval.position && size == p_rval.size;
+	}
+	constexpr bool operator!=(const AABB &p_rval) const {
+		return position != p_rval.position || size != p_rval.size;
+	}
+
+	bool is_equal_approx(const AABB &p_aabb) const;
+	bool is_same(const AABB &p_aabb) const;
+	bool is_finite() const;
+	_FORCE_INLINE_ bool intersects(const AABB &p_aabb) const; /// Both AABBs overlap
+	_FORCE_INLINE_ bool intersects_inclusive(const AABB &p_aabb) const; /// Both AABBs (or their faces) overlap
+	_FORCE_INLINE_ bool encloses(const AABB &p_aabb) const; /// p_aabb is completely inside this
+
+	AABB merge(const AABB &p_with) const;
+	void merge_with(const AABB &p_aabb); ///merge with another AABB
+	AABB intersection(const AABB &p_aabb) const; ///get box where two intersect, empty if no intersection occurs
+	_FORCE_INLINE_ bool smits_intersect_ray(const Vector3 &p_from, const Vector3 &p_dir, real_t p_t0, real_t p_t1) const;
+
+	bool intersects_segment(const Vector3 &p_from, const Vector3 &p_to, Vector3 *r_intersection_point = nullptr, Vector3 *r_normal = nullptr) const;
+	bool intersects_ray(const Vector3 &p_from, const Vector3 &p_dir) const {
+		bool inside;
+		return find_intersects_ray(p_from, p_dir, inside);
+	}
+	bool find_intersects_ray(const Vector3 &p_from, const Vector3 &p_dir, bool &r_inside, Vector3 *r_intersection_point = nullptr, Vector3 *r_normal = nullptr) const;
+
+	_FORCE_INLINE_ bool intersects_convex_shape(const Plane *p_planes, int p_plane_count, const Vector3 *p_points, int p_point_count) const;
+	_FORCE_INLINE_ bool inside_convex_shape(const Plane *p_planes, int p_plane_count) const;
+	bool intersects_plane(const Plane &p_plane) const;
+
+	_FORCE_INLINE_ bool has_point(const Vector3 &p_point) const;
+	_FORCE_INLINE_ Vector3 get_support(const Vector3 &p_direction) const;
+
+	Vector3 get_longest_axis() const;
+	int get_longest_axis_index() const;
+	_FORCE_INLINE_ real_t get_longest_axis_size() const;
+
+	Vector3 get_shortest_axis() const;
+	int get_shortest_axis_index() const;
+	_FORCE_INLINE_ real_t get_shortest_axis_size() const;
+
+	AABB grow(real_t p_by) const;
+	_FORCE_INLINE_ void grow_by(real_t p_amount);
+
+	void get_edge(int p_edge, Vector3 &r_from, Vector3 &r_to) const;
+	_FORCE_INLINE_ Vector3 get_endpoint(int p_point) const;
+
+	AABB expand(const Vector3 &p_vector) const;
+	_FORCE_INLINE_ void project_range_in_plane(const Plane &p_plane, real_t &r_min, real_t &r_max) const;
+	_FORCE_INLINE_ void expand_to(const Vector3 &p_vector); /** expand to contain a point if necessary */
+
+	_FORCE_INLINE_ AABB abs() const {
+		return AABB(position + size.minf(0), size.abs());
+	}
+
+	Variant intersects_segment_bind(const Vector3 &p_from, const Vector3 &p_to) const;
+	Variant intersects_ray_bind(const Vector3 &p_from, const Vector3 &p_dir) const;
+
+	_FORCE_INLINE_ void quantize(real_t p_unit);
+	_FORCE_INLINE_ AABB quantized(real_t p_unit) const;
+
+	_FORCE_INLINE_ void set_end(const Vector3 &p_end) {
+		size = p_end - position;
+	}
+
+	_FORCE_INLINE_ Vector3 get_end() const {
+		return position + size;
+	}
+
+	_FORCE_INLINE_ Vector3 get_center() const {
+		return position + (size * 0.5f);
+	}
+
+	explicit operator String() const;
+
+	AABB() = default;
+	constexpr AABB(const Vector3 &p_pos, const Vector3 &p_size) :
+			position(p_pos),
+			size(p_size) {
+	}
+};
+
+inline bool AABB::intersects(const AABB &p_aabb) const {
+#ifdef MATH_CHECKS
+	if (unlikely(size.x < 0 || size.y < 0 || size.z < 0 || p_aabb.size.x < 0 || p_aabb.size.y < 0 || p_aabb.size.z < 0)) {
+		ERR_PRINT("AABB size is negative, this is not supported. Use AABB.abs() to get an AABB with a positive size.");
+	}
+#endif
+	if (position.x >= (p_aabb.position.x + p_aabb.size.x)) {
+		return false;
+	}
+	if ((position.x + size.x) <= p_aabb.position.x) {
+		return false;
+	}
+	if (position.y >= (p_aabb.position.y + p_aabb.size.y)) {
+		return false;
+	}
+	if ((position.y + size.y) <= p_aabb.position.y) {
+		return false;
+	}
+	if (position.z >= (p_aabb.position.z + p_aabb.size.z)) {
+		return false;
+	}
+	if ((position.z + size.z) <= p_aabb.position.z) {
+		return false;
+	}
+
+	return true;
+}
+
+inline bool AABB::intersects_inclusive(const AABB &p_aabb) const {
+#ifdef MATH_CHECKS
+	if (unlikely(size.x < 0 || size.y < 0 || size.z < 0 || p_aabb.size.x < 0 || p_aabb.size.y < 0 || p_aabb.size.z < 0)) {
+		ERR_PRINT("AABB size is negative, this is not supported. Use AABB.abs() to get an AABB with a positive size.");
+	}
+#endif
+	if (position.x > (p_aabb.position.x + p_aabb.size.x)) {
+		return false;
+	}
+	if ((position.x + size.x) < p_aabb.position.x) {
+		return false;
+	}
+	if (position.y > (p_aabb.position.y + p_aabb.size.y)) {
+		return false;
+	}
+	if ((position.y + size.y) < p_aabb.position.y) {
+		return false;
+	}
+	if (position.z > (p_aabb.position.z + p_aabb.size.z)) {
+		return false;
+	}
+	if ((position.z + size.z) < p_aabb.position.z) {
+		return false;
+	}
+
+	return true;
+}
+
+inline bool AABB::encloses(const AABB &p_aabb) const {
+#ifdef MATH_CHECKS
+	if (unlikely(size.x < 0 || size.y < 0 || size.z < 0 || p_aabb.size.x < 0 || p_aabb.size.y < 0 || p_aabb.size.z < 0)) {
+		ERR_PRINT("AABB size is negative, this is not supported. Use AABB.abs() to get an AABB with a positive size.");
+	}
+#endif
+	Vector3 src_min = position;
+	Vector3 src_max = position + size;
+	Vector3 dst_min = p_aabb.position;
+	Vector3 dst_max = p_aabb.position + p_aabb.size;
+
+	return (
+			(src_min.x <= dst_min.x) &&
+			(src_max.x >= dst_max.x) &&
+			(src_min.y <= dst_min.y) &&
+			(src_max.y >= dst_max.y) &&
+			(src_min.z <= dst_min.z) &&
+			(src_max.z >= dst_max.z));
+}
+
+Vector3 AABB::get_support(const Vector3 &p_direction) const {
+	Vector3 support = position;
+	if (p_direction.x > 0.0f) {
+		support.x += size.x;
+	}
+	if (p_direction.y > 0.0f) {
+		support.y += size.y;
+	}
+	if (p_direction.z > 0.0f) {
+		support.z += size.z;
+	}
+	return support;
+}
+
+Vector3 AABB::get_endpoint(int p_point) const {
+	switch (p_point) {
+		case 0:
+			return Vector3(position.x, position.y, position.z);
+		case 1:
+			return Vector3(position.x, position.y, position.z + size.z);
+		case 2:
+			return Vector3(position.x, position.y + size.y, position.z);
+		case 3:
+			return Vector3(position.x, position.y + size.y, position.z + size.z);
+		case 4:
+			return Vector3(position.x + size.x, position.y, position.z);
+		case 5:
+			return Vector3(position.x + size.x, position.y, position.z + size.z);
+		case 6:
+			return Vector3(position.x + size.x, position.y + size.y, position.z);
+		case 7:
+			return Vector3(position.x + size.x, position.y + size.y, position.z + size.z);
+	}
+
+	ERR_FAIL_V(Vector3());
+}
+
+bool AABB::intersects_convex_shape(const Plane *p_planes, int p_plane_count, const Vector3 *p_points, int p_point_count) const {
+	Vector3 half_extents = size * 0.5f;
+	Vector3 ofs = position + half_extents;
+
+	for (int i = 0; i < p_plane_count; i++) {
+		const Plane &p = p_planes[i];
+		Vector3 point(
+				(p.normal.x > 0) ? -half_extents.x : half_extents.x,
+				(p.normal.y > 0) ? -half_extents.y : half_extents.y,
+				(p.normal.z > 0) ? -half_extents.z : half_extents.z);
+		point += ofs;
+		if (p.is_point_over(point)) {
+			return false;
+		}
+	}
+
+	// Make sure all points in the shape aren't fully separated from the AABB on
+	// each axis.
+	int bad_point_counts_positive[3] = { 0 };
+	int bad_point_counts_negative[3] = { 0 };
+
+	for (int k = 0; k < 3; k++) {
+		for (int i = 0; i < p_point_count; i++) {
+			if (p_points[i].coord[k] > ofs.coord[k] + half_extents.coord[k]) {
+				bad_point_counts_positive[k]++;
+			}
+			if (p_points[i].coord[k] < ofs.coord[k] - half_extents.coord[k]) {
+				bad_point_counts_negative[k]++;
+			}
+		}
+
+		if (bad_point_counts_negative[k] == p_point_count) {
+			return false;
+		}
+		if (bad_point_counts_positive[k] == p_point_count) {
+			return false;
+		}
+	}
+
+	return true;
+}
+
+bool AABB::inside_convex_shape(const Plane *p_planes, int p_plane_count) const {
+	Vector3 half_extents = size * 0.5f;
+	Vector3 ofs = position + half_extents;
+
+	for (int i = 0; i < p_plane_count; i++) {
+		const Plane &p = p_planes[i];
+		Vector3 point(
+				(p.normal.x < 0) ? -half_extents.x : half_extents.x,
+				(p.normal.y < 0) ? -half_extents.y : half_extents.y,
+				(p.normal.z < 0) ? -half_extents.z : half_extents.z);
+		point += ofs;
+		if (p.is_point_over(point)) {
+			return false;
+		}
+	}
+
+	return true;
+}
+
+bool AABB::has_point(const Vector3 &p_point) const {
+#ifdef MATH_CHECKS
+	if (unlikely(size.x < 0 || size.y < 0 || size.z < 0)) {
+		ERR_PRINT("AABB size is negative, this is not supported. Use AABB.abs() to get an AABB with a positive size.");
+	}
+#endif
+	if (p_point.x < position.x) {
+		return false;
+	}
+	if (p_point.y < position.y) {
+		return false;
+	}
+	if (p_point.z < position.z) {
+		return false;
+	}
+	if (p_point.x > position.x + size.x) {
+		return false;
+	}
+	if (p_point.y > position.y + size.y) {
+		return false;
+	}
+	if (p_point.z > position.z + size.z) {
+		return false;
+	}
+
+	return true;
+}
+
+inline void AABB::expand_to(const Vector3 &p_vector) {
+#ifdef MATH_CHECKS
+	if (unlikely(size.x < 0 || size.y < 0 || size.z < 0)) {
+		ERR_PRINT("AABB size is negative, this is not supported. Use AABB.abs() to get an AABB with a positive size.");
+	}
+#endif
+	Vector3 begin = position;
+	Vector3 end = position + size;
+
+	if (p_vector.x < begin.x) {
+		begin.x = p_vector.x;
+	}
+	if (p_vector.y < begin.y) {
+		begin.y = p_vector.y;
+	}
+	if (p_vector.z < begin.z) {
+		begin.z = p_vector.z;
+	}
+
+	if (p_vector.x > end.x) {
+		end.x = p_vector.x;
+	}
+	if (p_vector.y > end.y) {
+		end.y = p_vector.y;
+	}
+	if (p_vector.z > end.z) {
+		end.z = p_vector.z;
+	}
+
+	position = begin;
+	size = end - begin;
+}
+
+void AABB::project_range_in_plane(const Plane &p_plane, real_t &r_min, real_t &r_max) const {
+	Vector3 half_extents(size.x * 0.5f, size.y * 0.5f, size.z * 0.5f);
+	Vector3 center(position.x + half_extents.x, position.y + half_extents.y, position.z + half_extents.z);
+
+	real_t length = p_plane.normal.abs().dot(half_extents);
+	real_t distance = p_plane.distance_to(center);
+	r_min = distance - length;
+	r_max = distance + length;
+}
+
+inline real_t AABB::get_longest_axis_size() const {
+	real_t max_size = size.x;
+
+	if (size.y > max_size) {
+		max_size = size.y;
+	}
+
+	if (size.z > max_size) {
+		max_size = size.z;
+	}
+
+	return max_size;
+}
+
+inline real_t AABB::get_shortest_axis_size() const {
+	real_t max_size = size.x;
+
+	if (size.y < max_size) {
+		max_size = size.y;
+	}
+
+	if (size.z < max_size) {
+		max_size = size.z;
+	}
+
+	return max_size;
+}
+
+bool AABB::smits_intersect_ray(const Vector3 &p_from, const Vector3 &p_dir, real_t p_t0, real_t p_t1) const {
+#ifdef MATH_CHECKS
+	if (unlikely(size.x < 0 || size.y < 0 || size.z < 0)) {
+		ERR_PRINT("AABB size is negative, this is not supported. Use AABB.abs() to get an AABB with a positive size.");
+	}
+#endif
+	real_t divx = 1.0f / p_dir.x;
+	real_t divy = 1.0f / p_dir.y;
+	real_t divz = 1.0f / p_dir.z;
+
+	Vector3 upbound = position + size;
+	real_t tmin, tmax, tymin, tymax, tzmin, tzmax;
+	if (p_dir.x >= 0) {
+		tmin = (position.x - p_from.x) * divx;
+		tmax = (upbound.x - p_from.x) * divx;
+	} else {
+		tmin = (upbound.x - p_from.x) * divx;
+		tmax = (position.x - p_from.x) * divx;
+	}
+	if (p_dir.y >= 0) {
+		tymin = (position.y - p_from.y) * divy;
+		tymax = (upbound.y - p_from.y) * divy;
+	} else {
+		tymin = (upbound.y - p_from.y) * divy;
+		tymax = (position.y - p_from.y) * divy;
+	}
+	if ((tmin > tymax) || (tymin > tmax)) {
+		return false;
+	}
+	if (tymin > tmin) {
+		tmin = tymin;
+	}
+	if (tymax < tmax) {
+		tmax = tymax;
+	}
+	if (p_dir.z >= 0) {
+		tzmin = (position.z - p_from.z) * divz;
+		tzmax = (upbound.z - p_from.z) * divz;
+	} else {
+		tzmin = (upbound.z - p_from.z) * divz;
+		tzmax = (position.z - p_from.z) * divz;
+	}
+	if ((tmin > tzmax) || (tzmin > tmax)) {
+		return false;
+	}
+	if (tzmin > tmin) {
+		tmin = tzmin;
+	}
+	if (tzmax < tmax) {
+		tmax = tzmax;
+	}
+	return ((tmin < p_t1) && (tmax > p_t0));
+}
+
+void AABB::grow_by(real_t p_amount) {
+	position.x -= p_amount;
+	position.y -= p_amount;
+	position.z -= p_amount;
+	size.x += 2.0f * p_amount;
+	size.y += 2.0f * p_amount;
+	size.z += 2.0f * p_amount;
+}
+
+void AABB::quantize(real_t p_unit) {
+	size += position;
+
+	position.x -= Math::fposmodp(position.x, p_unit);
+	position.y -= Math::fposmodp(position.y, p_unit);
+	position.z -= Math::fposmodp(position.z, p_unit);
+
+	size.x -= Math::fposmodp(size.x, p_unit);
+	size.y -= Math::fposmodp(size.y, p_unit);
+	size.z -= Math::fposmodp(size.z, p_unit);
+
+	size.x += p_unit;
+	size.y += p_unit;
+	size.z += p_unit;
+
+	size -= position;
+}
+
+AABB AABB::quantized(real_t p_unit) const {
+	AABB ret = *this;
+	ret.quantize(p_unit);
+	return ret;
+}
+
+template <>
+struct is_zero_constructible<AABB> : std::true_type {};

core/math/audio_frame.h

@@ -0,0 +1,174 @@
+/**************************************************************************/
+/*  audio_frame.h                                                         */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "core/math/vector2.h"
+#include "core/typedefs.h"
+
+static _FORCE_INLINE_ float undenormalize(float f) {
+	union {
+		uint32_t i;
+		float f;
+	} v;
+
+	v.f = f;
+
+	// original: return (v.i & 0x7f800000) == 0 ? 0.0f : f;
+	// version from Tim Blechmann:
+	return (v.i & 0x7f800000) < 0x08000000 ? 0.0f : f;
+}
+
+static const float AUDIO_PEAK_OFFSET = 0.0000000001f;
+static const float AUDIO_MIN_PEAK_DB = -200.0f; // linear_to_db(AUDIO_PEAK_OFFSET)
+
+struct AudioFrame {
+	// Left and right samples.
+	union {
+		// NOLINTBEGIN(modernize-use-default-member-init)
+		struct {
+			float left;
+			float right;
+		};
+#ifndef DISABLE_DEPRECATED
+		struct {
+			float l;
+			float r;
+		};
+#endif
+		float levels[2] = { 0.0 };
+		// NOLINTEND(modernize-use-default-member-init)
+	};
+
+	_ALWAYS_INLINE_ const float &operator[](int p_idx) const {
+		DEV_ASSERT((unsigned int)p_idx < 2);
+		return levels[p_idx];
+	}
+	_ALWAYS_INLINE_ float &operator[](int p_idx) {
+		DEV_ASSERT((unsigned int)p_idx < 2);
+		return levels[p_idx];
+	}
+
+	constexpr AudioFrame operator+(const AudioFrame &p_frame) const { return AudioFrame(left + p_frame.left, right + p_frame.right); }
+	constexpr AudioFrame operator-(const AudioFrame &p_frame) const { return AudioFrame(left - p_frame.left, right - p_frame.right); }
+	constexpr AudioFrame operator*(const AudioFrame &p_frame) const { return AudioFrame(left * p_frame.left, right * p_frame.right); }
+	constexpr AudioFrame operator/(const AudioFrame &p_frame) const { return AudioFrame(left / p_frame.left, right / p_frame.right); }
+
+	constexpr AudioFrame operator+(float p_sample) const { return AudioFrame(left + p_sample, right + p_sample); }
+	constexpr AudioFrame operator-(float p_sample) const { return AudioFrame(left - p_sample, right - p_sample); }
+	constexpr AudioFrame operator*(float p_sample) const { return AudioFrame(left * p_sample, right * p_sample); }
+	constexpr AudioFrame operator/(float p_sample) const { return AudioFrame(left / p_sample, right / p_sample); }
+
+	constexpr void operator+=(const AudioFrame &p_frame) {
+		left += p_frame.left;
+		right += p_frame.right;
+	}
+	constexpr void operator-=(const AudioFrame &p_frame) {
+		left -= p_frame.left;
+		right -= p_frame.right;
+	}
+	constexpr void operator*=(const AudioFrame &p_frame) {
+		left *= p_frame.left;
+		right *= p_frame.right;
+	}
+	constexpr void operator/=(const AudioFrame &p_frame) {
+		left /= p_frame.left;
+		right /= p_frame.right;
+	}
+
+	constexpr void operator+=(float p_sample) {
+		left += p_sample;
+		right += p_sample;
+	}
+	constexpr void operator-=(float p_sample) {
+		left -= p_sample;
+		right -= p_sample;
+	}
+	constexpr void operator*=(float p_sample) {
+		left *= p_sample;
+		right *= p_sample;
+	}
+	constexpr void operator/=(float p_sample) {
+		left /= p_sample;
+		right /= p_sample;
+	}
+
+	_ALWAYS_INLINE_ void undenormalize() {
+		left = ::undenormalize(left);
+		right = ::undenormalize(right);
+	}
+
+	_FORCE_INLINE_ AudioFrame lerp(const AudioFrame &p_b, float p_t) const {
+		AudioFrame res = *this;
+
+		res.left += (p_t * (p_b.left - left));
+		res.right += (p_t * (p_b.right - right));
+
+		return res;
+	}
+
+	// NOLINTBEGIN(cppcoreguidelines-pro-type-member-init)
+	constexpr AudioFrame(float p_left, float p_right) :
+			left(p_left), right(p_right) {}
+	constexpr AudioFrame(const AudioFrame &p_frame) :
+			left(p_frame.left), right(p_frame.right) {}
+	// NOLINTEND(cppcoreguidelines-pro-type-member-init)
+
+	constexpr void operator=(const AudioFrame &p_frame) {
+		left = p_frame.left;
+		right = p_frame.right;
+	}
+
+	constexpr operator Vector2() const {
+		return Vector2(left, right);
+	}
+
+	// NOLINTBEGIN(cppcoreguidelines-pro-type-member-init)
+	constexpr AudioFrame(const Vector2 &p_v2) :
+			left(p_v2.x), right(p_v2.y) {}
+	constexpr AudioFrame() :
+			left(0), right(0) {}
+	// NOLINTEND(cppcoreguidelines-pro-type-member-init)
+};
+
+constexpr AudioFrame operator*(float p_scalar, const AudioFrame &p_frame) {
+	return AudioFrame(p_frame.left * p_scalar, p_frame.right * p_scalar);
+}
+
+constexpr AudioFrame operator*(int32_t p_scalar, const AudioFrame &p_frame) {
+	return AudioFrame(p_frame.left * p_scalar, p_frame.right * p_scalar);
+}
+
+constexpr AudioFrame operator*(int64_t p_scalar, const AudioFrame &p_frame) {
+	return AudioFrame(p_frame.left * p_scalar, p_frame.right * p_scalar);
+}
+
+template <>
+struct is_zero_constructible<AudioFrame> : std::true_type {};

core/math/basis.h

@@ -0,0 +1,346 @@
+/**************************************************************************/
+/*  basis.h                                                               */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "core/math/quaternion.h"
+#include "core/math/vector3.h"
+
+struct [[nodiscard]] Basis {
+	Vector3 rows[3] = {
+		Vector3(1, 0, 0),
+		Vector3(0, 1, 0),
+		Vector3(0, 0, 1)
+	};
+
+	constexpr const Vector3 &operator[](int p_row) const {
+		return rows[p_row];
+	}
+	constexpr Vector3 &operator[](int p_row) {
+		return rows[p_row];
+	}
+
+	void invert();
+	void transpose();
+
+	Basis inverse() const;
+	Basis transposed() const;
+
+	_FORCE_INLINE_ real_t determinant() const;
+
+	void rotate(const Vector3 &p_axis, real_t p_angle);
+	Basis rotated(const Vector3 &p_axis, real_t p_angle) const;
+
+	void rotate_local(const Vector3 &p_axis, real_t p_angle);
+	Basis rotated_local(const Vector3 &p_axis, real_t p_angle) const;
+
+	void rotate(const Vector3 &p_euler, EulerOrder p_order = EulerOrder::YXZ);
+	Basis rotated(const Vector3 &p_euler, EulerOrder p_order = EulerOrder::YXZ) const;
+
+	void rotate(const Quaternion &p_quaternion);
+	Basis rotated(const Quaternion &p_quaternion) const;
+
+	Vector3 get_euler_normalized(EulerOrder p_order = EulerOrder::YXZ) const;
+	void get_rotation_axis_angle(Vector3 &p_axis, real_t &p_angle) const;
+	void get_rotation_axis_angle_local(Vector3 &p_axis, real_t &p_angle) const;
+	Quaternion get_rotation_quaternion() const;
+
+	void rotate_to_align(Vector3 p_start_direction, Vector3 p_end_direction);
+
+	Vector3 rotref_posscale_decomposition(Basis &rotref) const;
+
+	Vector3 get_euler(EulerOrder p_order = EulerOrder::YXZ) const;
+	void set_euler(const Vector3 &p_euler, EulerOrder p_order = EulerOrder::YXZ);
+	static Basis from_euler(const Vector3 &p_euler, EulerOrder p_order = EulerOrder::YXZ) {
+		Basis b;
+		b.set_euler(p_euler, p_order);
+		return b;
+	}
+
+	Quaternion get_quaternion() const;
+	void set_quaternion(const Quaternion &p_quaternion);
+
+	void get_axis_angle(Vector3 &r_axis, real_t &r_angle) const;
+	void set_axis_angle(const Vector3 &p_axis, real_t p_angle);
+
+	void scale(const Vector3 &p_scale);
+	Basis scaled(const Vector3 &p_scale) const;
+
+	void scale_local(const Vector3 &p_scale);
+	Basis scaled_local(const Vector3 &p_scale) const;
+
+	void scale_orthogonal(const Vector3 &p_scale);
+	Basis scaled_orthogonal(const Vector3 &p_scale) const;
+	real_t get_uniform_scale() const;
+
+	Vector3 get_scale() const;
+	Vector3 get_scale_abs() const;
+	Vector3 get_scale_global() const;
+
+	void set_axis_angle_scale(const Vector3 &p_axis, real_t p_angle, const Vector3 &p_scale);
+	void set_euler_scale(const Vector3 &p_euler, const Vector3 &p_scale, EulerOrder p_order = EulerOrder::YXZ);
+	void set_quaternion_scale(const Quaternion &p_quaternion, const Vector3 &p_scale);
+
+	// transposed dot products
+	_FORCE_INLINE_ real_t tdotx(const Vector3 &p_v) const {
+		return rows[0][0] * p_v[0] + rows[1][0] * p_v[1] + rows[2][0] * p_v[2];
+	}
+	_FORCE_INLINE_ real_t tdoty(const Vector3 &p_v) const {
+		return rows[0][1] * p_v[0] + rows[1][1] * p_v[1] + rows[2][1] * p_v[2];
+	}
+	_FORCE_INLINE_ real_t tdotz(const Vector3 &p_v) const {
+		return rows[0][2] * p_v[0] + rows[1][2] * p_v[1] + rows[2][2] * p_v[2];
+	}
+
+	bool is_equal_approx(const Basis &p_basis) const;
+	bool is_same(const Basis &p_basis) const;
+	bool is_finite() const;
+
+	constexpr bool operator==(const Basis &p_matrix) const;
+	constexpr bool operator!=(const Basis &p_matrix) const;
+
+	_FORCE_INLINE_ Vector3 xform(const Vector3 &p_vector) const;
+	_FORCE_INLINE_ Vector3 xform_inv(const Vector3 &p_vector) const;
+	_FORCE_INLINE_ void operator*=(const Basis &p_matrix);
+	_FORCE_INLINE_ Basis operator*(const Basis &p_matrix) const;
+	constexpr void operator+=(const Basis &p_matrix);
+	constexpr Basis operator+(const Basis &p_matrix) const;
+	constexpr void operator-=(const Basis &p_matrix);
+	constexpr Basis operator-(const Basis &p_matrix) const;
+	constexpr void operator*=(real_t p_val);
+	constexpr Basis operator*(real_t p_val) const;
+	constexpr void operator/=(real_t p_val);
+	constexpr Basis operator/(real_t p_val) const;
+
+	bool is_orthogonal() const;
+	bool is_orthonormal() const;
+	bool is_conformal() const;
+	bool is_diagonal() const;
+	bool is_rotation() const;
+
+	Basis lerp(const Basis &p_to, real_t p_weight) const;
+	Basis slerp(const Basis &p_to, real_t p_weight) const;
+	void rotate_sh(real_t *p_values);
+
+	explicit operator String() const;
+
+	/* create / set */
+
+	_FORCE_INLINE_ void set(real_t p_xx, real_t p_xy, real_t p_xz, real_t p_yx, real_t p_yy, real_t p_yz, real_t p_zx, real_t p_zy, real_t p_zz) {
+		rows[0][0] = p_xx;
+		rows[0][1] = p_xy;
+		rows[0][2] = p_xz;
+		rows[1][0] = p_yx;
+		rows[1][1] = p_yy;
+		rows[1][2] = p_yz;
+		rows[2][0] = p_zx;
+		rows[2][1] = p_zy;
+		rows[2][2] = p_zz;
+	}
+	_FORCE_INLINE_ void set_columns(const Vector3 &p_x, const Vector3 &p_y, const Vector3 &p_z) {
+		set_column(0, p_x);
+		set_column(1, p_y);
+		set_column(2, p_z);
+	}
+
+	_FORCE_INLINE_ Vector3 get_column(int p_index) const {
+		// Get actual basis axis column (we store transposed as rows for performance).
+		return Vector3(rows[0][p_index], rows[1][p_index], rows[2][p_index]);
+	}
+
+	_FORCE_INLINE_ void set_column(int p_index, const Vector3 &p_value) {
+		// Set actual basis axis column (we store transposed as rows for performance).
+		rows[0][p_index] = p_value.x;
+		rows[1][p_index] = p_value.y;
+		rows[2][p_index] = p_value.z;
+	}
+
+	_FORCE_INLINE_ Vector3 get_main_diagonal() const {
+		return Vector3(rows[0][0], rows[1][1], rows[2][2]);
+	}
+
+	_FORCE_INLINE_ void set_zero() {
+		rows[0].zero();
+		rows[1].zero();
+		rows[2].zero();
+	}
+
+	_FORCE_INLINE_ Basis transpose_xform(const Basis &p_m) const {
+		return Basis(
+				rows[0].x * p_m[0].x + rows[1].x * p_m[1].x + rows[2].x * p_m[2].x,
+				rows[0].x * p_m[0].y + rows[1].x * p_m[1].y + rows[2].x * p_m[2].y,
+				rows[0].x * p_m[0].z + rows[1].x * p_m[1].z + rows[2].x * p_m[2].z,
+				rows[0].y * p_m[0].x + rows[1].y * p_m[1].x + rows[2].y * p_m[2].x,
+				rows[0].y * p_m[0].y + rows[1].y * p_m[1].y + rows[2].y * p_m[2].y,
+				rows[0].y * p_m[0].z + rows[1].y * p_m[1].z + rows[2].y * p_m[2].z,
+				rows[0].z * p_m[0].x + rows[1].z * p_m[1].x + rows[2].z * p_m[2].x,
+				rows[0].z * p_m[0].y + rows[1].z * p_m[1].y + rows[2].z * p_m[2].y,
+				rows[0].z * p_m[0].z + rows[1].z * p_m[1].z + rows[2].z * p_m[2].z);
+	}
+	constexpr Basis(real_t p_xx, real_t p_xy, real_t p_xz, real_t p_yx, real_t p_yy, real_t p_yz, real_t p_zx, real_t p_zy, real_t p_zz) :
+			rows{
+				{ p_xx, p_xy, p_xz },
+				{ p_yx, p_yy, p_yz },
+				{ p_zx, p_zy, p_zz },
+			} {}
+
+	void orthonormalize();
+	Basis orthonormalized() const;
+
+	void orthogonalize();
+	Basis orthogonalized() const;
+
+#ifdef MATH_CHECKS
+	bool is_symmetric() const;
+#endif
+	Basis diagonalize();
+
+	operator Quaternion() const { return get_quaternion(); }
+
+	static Basis looking_at(const Vector3 &p_target, const Vector3 &p_up = Vector3(0, 1, 0), bool p_use_model_front = false);
+
+	Basis(const Quaternion &p_quaternion) { set_quaternion(p_quaternion); }
+	Basis(const Quaternion &p_quaternion, const Vector3 &p_scale) { set_quaternion_scale(p_quaternion, p_scale); }
+
+	Basis(const Vector3 &p_axis, real_t p_angle) { set_axis_angle(p_axis, p_angle); }
+	Basis(const Vector3 &p_axis, real_t p_angle, const Vector3 &p_scale) { set_axis_angle_scale(p_axis, p_angle, p_scale); }
+	static Basis from_scale(const Vector3 &p_scale);
+
+	constexpr Basis(const Vector3 &p_x_axis, const Vector3 &p_y_axis, const Vector3 &p_z_axis) :
+			rows{
+				{ p_x_axis.x, p_y_axis.x, p_z_axis.x },
+				{ p_x_axis.y, p_y_axis.y, p_z_axis.y },
+				{ p_x_axis.z, p_y_axis.z, p_z_axis.z },
+			} {}
+
+	Basis() = default;
+
+private:
+	// Helper method.
+	void _set_diagonal(const Vector3 &p_diag);
+};
+
+constexpr bool Basis::operator==(const Basis &p_matrix) const {
+	for (int i = 0; i < 3; i++) {
+		for (int j = 0; j < 3; j++) {
+			if (rows[i][j] != p_matrix.rows[i][j]) {
+				return false;
+			}
+		}
+	}
+
+	return true;
+}
+
+constexpr bool Basis::operator!=(const Basis &p_matrix) const {
+	return (!(*this == p_matrix));
+}
+
+_FORCE_INLINE_ void Basis::operator*=(const Basis &p_matrix) {
+	set(
+			p_matrix.tdotx(rows[0]), p_matrix.tdoty(rows[0]), p_matrix.tdotz(rows[0]),
+			p_matrix.tdotx(rows[1]), p_matrix.tdoty(rows[1]), p_matrix.tdotz(rows[1]),
+			p_matrix.tdotx(rows[2]), p_matrix.tdoty(rows[2]), p_matrix.tdotz(rows[2]));
+}
+
+_FORCE_INLINE_ Basis Basis::operator*(const Basis &p_matrix) const {
+	return Basis(
+			p_matrix.tdotx(rows[0]), p_matrix.tdoty(rows[0]), p_matrix.tdotz(rows[0]),
+			p_matrix.tdotx(rows[1]), p_matrix.tdoty(rows[1]), p_matrix.tdotz(rows[1]),
+			p_matrix.tdotx(rows[2]), p_matrix.tdoty(rows[2]), p_matrix.tdotz(rows[2]));
+}
+
+constexpr void Basis::operator+=(const Basis &p_matrix) {
+	rows[0] += p_matrix.rows[0];
+	rows[1] += p_matrix.rows[1];
+	rows[2] += p_matrix.rows[2];
+}
+
+constexpr Basis Basis::operator+(const Basis &p_matrix) const {
+	Basis ret(*this);
+	ret += p_matrix;
+	return ret;
+}
+
+constexpr void Basis::operator-=(const Basis &p_matrix) {
+	rows[0] -= p_matrix.rows[0];
+	rows[1] -= p_matrix.rows[1];
+	rows[2] -= p_matrix.rows[2];
+}
+
+constexpr Basis Basis::operator-(const Basis &p_matrix) const {
+	Basis ret(*this);
+	ret -= p_matrix;
+	return ret;
+}
+
+constexpr void Basis::operator*=(real_t p_val) {
+	rows[0] *= p_val;
+	rows[1] *= p_val;
+	rows[2] *= p_val;
+}
+
+constexpr Basis Basis::operator*(real_t p_val) const {
+	Basis ret(*this);
+	ret *= p_val;
+	return ret;
+}
+
+constexpr void Basis::operator/=(real_t p_val) {
+	rows[0] /= p_val;
+	rows[1] /= p_val;
+	rows[2] /= p_val;
+}
+
+constexpr Basis Basis::operator/(real_t p_val) const {
+	Basis ret(*this);
+	ret /= p_val;
+	return ret;
+}
+
+Vector3 Basis::xform(const Vector3 &p_vector) const {
+	return Vector3(
+			rows[0].dot(p_vector),
+			rows[1].dot(p_vector),
+			rows[2].dot(p_vector));
+}
+
+Vector3 Basis::xform_inv(const Vector3 &p_vector) const {
+	return Vector3(
+			(rows[0][0] * p_vector.x) + (rows[1][0] * p_vector.y) + (rows[2][0] * p_vector.z),
+			(rows[0][1] * p_vector.x) + (rows[1][1] * p_vector.y) + (rows[2][1] * p_vector.z),
+			(rows[0][2] * p_vector.x) + (rows[1][2] * p_vector.y) + (rows[2][2] * p_vector.z));
+}
+
+real_t Basis::determinant() const {
+	return rows[0][0] * (rows[1][1] * rows[2][2] - rows[2][1] * rows[1][2]) -
+			rows[1][0] * (rows[0][1] * rows[2][2] - rows[2][1] * rows[0][2]) +
+			rows[2][0] * (rows[0][1] * rows[1][2] - rows[1][1] * rows[0][2]);
+}

core/math/bvh.h

@@ -0,0 +1,807 @@
+/**************************************************************************/
+/*  bvh.h                                                                 */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#pragma once
+
+// BVH
+// This class provides a wrapper around BVH tree, which contains most of the functionality
+// for a dynamic BVH with templated leaf size.
+// However BVH also adds facilities for pairing, to maintain compatibility with Godot 3.2.
+// Pairing is a collision pairing system, on top of the basic BVH.
+
+// Some notes on the use of BVH / Octree from Godot 3.2.
+// This is not well explained elsewhere.
+// The rendering tree mask and types that are sent to the BVH are NOT layer masks.
+// They are INSTANCE_TYPES (defined in visual_server.h), e.g. MESH, MULTIMESH, PARTICLES etc.
+// Thus the lights do no cull by layer mask in the BVH.
+
+// Layer masks are implemented in the renderers as a later step, and light_cull_mask appears to be
+// implemented in GLES3 but not GLES2. Layer masks are not yet implemented for directional lights.
+
+// In the physics, the pairable_type is based on 1 << p_object->get_type() where:
+// TYPE_AREA,
+// TYPE_BODY
+// and pairable_mask is either 0 if static, or set to all if non static
+
+#include "bvh_tree.h"
+
+#include "core/math/geometry_3d.h"
+#include "core/os/mutex.h"
+
+#define BVHTREE_CLASS BVH_Tree<T, NUM_TREES, 2, MAX_ITEMS, USER_PAIR_TEST_FUNCTION, USER_CULL_TEST_FUNCTION, USE_PAIRS, BOUNDS, POINT>
+#define BVH_LOCKED_FUNCTION BVHLockedFunction _lock_guard(&_mutex, BVH_THREAD_SAFE &&_thread_safe);
+
+template <typename T, int NUM_TREES = 1, bool USE_PAIRS = false, int MAX_ITEMS = 32, typename USER_PAIR_TEST_FUNCTION = BVH_DummyPairTestFunction<T>, typename USER_CULL_TEST_FUNCTION = BVH_DummyCullTestFunction<T>, typename BOUNDS = AABB, typename POINT = Vector3, bool BVH_THREAD_SAFE = true>
+class BVH_Manager {
+public:
+	// note we are using uint32_t instead of BVHHandle, losing type safety, but this
+	// is for compatibility with octree
+	typedef void *(*PairCallback)(void *, uint32_t, T *, int, uint32_t, T *, int);
+	typedef void (*UnpairCallback)(void *, uint32_t, T *, int, uint32_t, T *, int, void *);
+	typedef void *(*CheckPairCallback)(void *, uint32_t, T *, int, uint32_t, T *, int, void *);
+
+	// allow locally toggling thread safety if the template has been compiled with BVH_THREAD_SAFE
+	void params_set_thread_safe(bool p_enable) {
+		_thread_safe = p_enable;
+	}
+
+	// these 2 are crucial for fine tuning, and can be applied manually
+	// see the variable declarations for more info.
+	void params_set_node_expansion(real_t p_value) {
+		BVH_LOCKED_FUNCTION
+		if (p_value >= 0.0) {
+			tree._node_expansion = p_value;
+			tree._auto_node_expansion = false;
+		} else {
+			tree._auto_node_expansion = true;
+		}
+	}
+
+	void params_set_pairing_expansion(real_t p_value) {
+		BVH_LOCKED_FUNCTION
+		tree.params_set_pairing_expansion(p_value);
+	}
+
+	void set_pair_callback(PairCallback p_callback, void *p_userdata) {
+		BVH_LOCKED_FUNCTION
+		pair_callback = p_callback;
+		pair_callback_userdata = p_userdata;
+	}
+	void set_unpair_callback(UnpairCallback p_callback, void *p_userdata) {
+		BVH_LOCKED_FUNCTION
+		unpair_callback = p_callback;
+		unpair_callback_userdata = p_userdata;
+	}
+	void set_check_pair_callback(CheckPairCallback p_callback, void *p_userdata) {
+		BVH_LOCKED_FUNCTION
+		check_pair_callback = p_callback;
+		check_pair_callback_userdata = p_userdata;
+	}
+
+	BVHHandle create(T *p_userdata, bool p_active = true, uint32_t p_tree_id = 0, uint32_t p_tree_collision_mask = 1, const BOUNDS &p_aabb = BOUNDS(), int p_subindex = 0) {
+		BVH_LOCKED_FUNCTION
+
+		// not sure if absolutely necessary to flush collisions here. It will cost performance to, instead
+		// of waiting for update, so only uncomment this if there are bugs.
+		if (USE_PAIRS) {
+			//_check_for_collisions();
+		}
+
+		BVHHandle h = tree.item_add(p_userdata, p_active, p_aabb, p_subindex, p_tree_id, p_tree_collision_mask);
+
+		if (USE_PAIRS) {
+			// for safety initialize the expanded AABB
+			BOUNDS &expanded_aabb = tree._pairs[h.id()].expanded_aabb;
+			expanded_aabb = p_aabb;
+			expanded_aabb.grow_by(tree._pairing_expansion);
+
+			// force a collision check no matter the AABB
+			if (p_active) {
+				_add_changed_item(h, p_aabb, false);
+				_check_for_collisions(true);
+			}
+		}
+
+		return h;
+	}
+
+	////////////////////////////////////////////////////
+	// wrapper versions that use uint32_t instead of handle
+	// for backward compatibility. Less type safe
+	void move(uint32_t p_handle, const BOUNDS &p_aabb) {
+		BVHHandle h;
+		h.set(p_handle);
+		move(h, p_aabb);
+	}
+
+	void recheck_pairs(uint32_t p_handle) {
+		BVHHandle h;
+		h.set(p_handle);
+		recheck_pairs(h);
+	}
+
+	void erase(uint32_t p_handle) {
+		BVHHandle h;
+		h.set(p_handle);
+		erase(h);
+	}
+
+	void force_collision_check(uint32_t p_handle) {
+		BVHHandle h;
+		h.set(p_handle);
+		force_collision_check(h);
+	}
+
+	bool activate(uint32_t p_handle, const BOUNDS &p_aabb, bool p_delay_collision_check = false) {
+		BVHHandle h;
+		h.set(p_handle);
+		return activate(h, p_aabb, p_delay_collision_check);
+	}
+
+	bool deactivate(uint32_t p_handle) {
+		BVHHandle h;
+		h.set(p_handle);
+		return deactivate(h);
+	}
+
+	void set_tree(uint32_t p_handle, uint32_t p_tree_id, uint32_t p_tree_collision_mask, bool p_force_collision_check = true) {
+		BVHHandle h;
+		h.set(p_handle);
+		set_tree(h, p_tree_id, p_tree_collision_mask, p_force_collision_check);
+	}
+
+	uint32_t get_tree_id(uint32_t p_handle) const {
+		BVHHandle h;
+		h.set(p_handle);
+		return item_get_tree_id(h);
+	}
+	int get_subindex(uint32_t p_handle) const {
+		BVHHandle h;
+		h.set(p_handle);
+		return item_get_subindex(h);
+	}
+
+	T *get(uint32_t p_handle) const {
+		BVHHandle h;
+		h.set(p_handle);
+		return item_get_userdata(h);
+	}
+
+	////////////////////////////////////////////////////
+
+	void move(BVHHandle p_handle, const BOUNDS &p_aabb) {
+		DEV_ASSERT(!p_handle.is_invalid());
+		BVH_LOCKED_FUNCTION
+		if (tree.item_move(p_handle, p_aabb)) {
+			if (USE_PAIRS) {
+				_add_changed_item(p_handle, p_aabb);
+			}
+		}
+	}
+
+	void recheck_pairs(BVHHandle p_handle) {
+		DEV_ASSERT(!p_handle.is_invalid());
+		force_collision_check(p_handle);
+	}
+
+	void erase(BVHHandle p_handle) {
+		DEV_ASSERT(!p_handle.is_invalid());
+		BVH_LOCKED_FUNCTION
+		// call unpair and remove all references to the item
+		// before deleting from the tree
+		if (USE_PAIRS) {
+			_remove_changed_item(p_handle);
+		}
+
+		tree.item_remove(p_handle);
+
+		_check_for_collisions(true);
+	}
+
+	// use in conjunction with activate if you have deferred the collision check, and
+	// set pairable has never been called.
+	// (deferred collision checks are a workaround for visual server for historical reasons)
+	void force_collision_check(BVHHandle p_handle) {
+		DEV_ASSERT(!p_handle.is_invalid());
+		BVH_LOCKED_FUNCTION
+		if (USE_PAIRS) {
+			// the aabb should already be up to date in the BVH
+			BOUNDS aabb;
+			item_get_AABB(p_handle, aabb);
+
+			// add it as changed even if aabb not different
+			_add_changed_item(p_handle, aabb, false);
+
+			// force an immediate full collision check, much like calls to set_pairable
+			_check_for_collisions(true);
+		}
+	}
+
+	// these should be read as set_visible for render trees,
+	// but generically this makes items add or remove from the
+	// tree internally, to speed things up by ignoring inactive items
+	bool activate(BVHHandle p_handle, const BOUNDS &p_aabb, bool p_delay_collision_check = false) {
+		DEV_ASSERT(!p_handle.is_invalid());
+		BVH_LOCKED_FUNCTION
+		// sending the aabb here prevents the need for the BVH to maintain
+		// a redundant copy of the aabb.
+		// returns success
+		if (tree.item_activate(p_handle, p_aabb)) {
+			if (USE_PAIRS) {
+				// in the special case of the render tree, when setting visibility we are using the combination of
+				// activate then set_pairable. This would case 2 sets of collision checks. For efficiency here we allow
+				// deferring to have a single collision check at the set_pairable call.
+				// Watch for bugs! This may cause bugs if set_pairable is not called.
+				if (!p_delay_collision_check) {
+					_add_changed_item(p_handle, p_aabb, false);
+
+					// force an immediate collision check, much like calls to set_pairable
+					_check_for_collisions(true);
+				}
+			}
+			return true;
+		}
+
+		return false;
+	}
+
+	bool deactivate(BVHHandle p_handle) {
+		DEV_ASSERT(!p_handle.is_invalid());
+		BVH_LOCKED_FUNCTION
+		// returns success
+		if (tree.item_deactivate(p_handle)) {
+			// call unpair and remove all references to the item
+			// before deleting from the tree
+			if (USE_PAIRS) {
+				_remove_changed_item(p_handle);
+
+				// force check for collisions, much like an erase was called
+				_check_for_collisions(true);
+			}
+			return true;
+		}
+
+		return false;
+	}
+
+	bool get_active(BVHHandle p_handle) {
+		DEV_ASSERT(!p_handle.is_invalid());
+		BVH_LOCKED_FUNCTION
+		return tree.item_get_active(p_handle);
+	}
+
+	// call e.g. once per frame (this does a trickle optimize)
+	void update() {
+		BVH_LOCKED_FUNCTION
+		tree.update();
+		_check_for_collisions();
+#ifdef BVH_INTEGRITY_CHECKS
+		tree._integrity_check_all();
+#endif
+	}
+
+	// this can be called more frequently than per frame if necessary
+	void update_collisions() {
+		BVH_LOCKED_FUNCTION
+		_check_for_collisions();
+	}
+
+	// prefer calling this directly as type safe
+	void set_tree(const BVHHandle &p_handle, uint32_t p_tree_id, uint32_t p_tree_collision_mask, bool p_force_collision_check = true) {
+		DEV_ASSERT(!p_handle.is_invalid());
+		BVH_LOCKED_FUNCTION
+		// Returns true if the pairing state has changed.
+		bool state_changed = tree.item_set_tree(p_handle, p_tree_id, p_tree_collision_mask);
+
+		if (USE_PAIRS) {
+			// not sure if absolutely necessary to flush collisions here. It will cost performance to, instead
+			// of waiting for update, so only uncomment this if there are bugs.
+			//_check_for_collisions();
+
+			if ((p_force_collision_check || state_changed) && tree.item_get_active(p_handle)) {
+				// when the pairable state changes, we need to force a collision check because newly pairable
+				// items may be in collision, and unpairable items might move out of collision.
+				// We cannot depend on waiting for the next update, because that may come much later.
+				BOUNDS aabb;
+				item_get_AABB(p_handle, aabb);
+
+				// passing false disables the optimization which prevents collision checks if
+				// the aabb hasn't changed
+				_add_changed_item(p_handle, aabb, false);
+
+				// force an immediate collision check (probably just for this one item)
+				// but it must be a FULL collision check, also checking pairable state and masks.
+				// This is because AABB intersecting objects may have changed pairable state / mask
+				// such that they should no longer be paired. E.g. lights.
+				_check_for_collisions(true);
+			} // only if active
+		}
+	}
+
+	// cull tests
+	int cull_aabb(const BOUNDS &p_aabb, T **p_result_array, int p_result_max, const T *p_tester, uint32_t p_tree_collision_mask = 0xFFFFFFFF, int *p_subindex_array = nullptr) {
+		BVH_LOCKED_FUNCTION
+		typename BVHTREE_CLASS::CullParams params;
+
+		params.result_count_overall = 0;
+		params.result_max = p_result_max;
+		params.result_array = p_result_array;
+		params.subindex_array = p_subindex_array;
+		params.tree_collision_mask = p_tree_collision_mask;
+		params.abb.from(p_aabb);
+		params.tester = p_tester;
+
+		tree.cull_aabb(params);
+
+		return params.result_count_overall;
+	}
+
+	int cull_segment(const POINT &p_from, const POINT &p_to, T **p_result_array, int p_result_max, const T *p_tester, uint32_t p_tree_collision_mask = 0xFFFFFFFF, int *p_subindex_array = nullptr) {
+		BVH_LOCKED_FUNCTION
+		typename BVHTREE_CLASS::CullParams params;
+
+		params.result_count_overall = 0;
+		params.result_max = p_result_max;
+		params.result_array = p_result_array;
+		params.subindex_array = p_subindex_array;
+		params.tester = p_tester;
+		params.tree_collision_mask = p_tree_collision_mask;
+
+		params.segment.from = p_from;
+		params.segment.to = p_to;
+
+		tree.cull_segment(params);
+
+		return params.result_count_overall;
+	}
+
+	int cull_point(const POINT &p_point, T **p_result_array, int p_result_max, const T *p_tester, uint32_t p_tree_collision_mask = 0xFFFFFFFF, int *p_subindex_array = nullptr) {
+		BVH_LOCKED_FUNCTION
+		typename BVHTREE_CLASS::CullParams params;
+
+		params.result_count_overall = 0;
+		params.result_max = p_result_max;
+		params.result_array = p_result_array;
+		params.subindex_array = p_subindex_array;
+		params.tester = p_tester;
+		params.tree_collision_mask = p_tree_collision_mask;
+
+		params.point = p_point;
+
+		tree.cull_point(params);
+		return params.result_count_overall;
+	}
+
+	int cull_convex(const Vector<Plane> &p_convex, T **p_result_array, int p_result_max, const T *p_tester, uint32_t p_tree_collision_mask = 0xFFFFFFFF) {
+		BVH_LOCKED_FUNCTION
+		if (!p_convex.size()) {
+			return 0;
+		}
+
+		Vector<Vector3> convex_points = Geometry3D::compute_convex_mesh_points(&p_convex[0], p_convex.size());
+		if (convex_points.is_empty()) {
+			return 0;
+		}
+
+		typename BVHTREE_CLASS::CullParams params;
+		params.result_count_overall = 0;
+		params.result_max = p_result_max;
+		params.result_array = p_result_array;
+		params.subindex_array = nullptr;
+		params.tester = p_tester;
+		params.tree_collision_mask = p_tree_collision_mask;
+
+		params.hull.planes = &p_convex[0];
+		params.hull.num_planes = p_convex.size();
+		params.hull.points = &convex_points[0];
+		params.hull.num_points = convex_points.size();
+
+		tree.cull_convex(params);
+
+		return params.result_count_overall;
+	}
+
+private:
+	// do this after moving etc.
+	void _check_for_collisions(bool p_full_check = false) {
+		if (!changed_items.size()) {
+			// noop
+			return;
+		}
+
+		typename BVHTREE_CLASS::CullParams params;
+
+		params.result_count_overall = 0;
+		params.result_max = INT_MAX;
+		params.result_array = nullptr;
+		params.subindex_array = nullptr;
+
+		for (const BVHHandle &h : changed_items) {
+			// use the expanded aabb for pairing
+			const BOUNDS &expanded_aabb = tree._pairs[h.id()].expanded_aabb;
+			BVHABB_CLASS abb;
+			abb.from(expanded_aabb);
+
+			tree.item_fill_cullparams(h, params);
+
+			// find all the existing paired aabbs that are no longer
+			// paired, and send callbacks
+			_find_leavers(h, abb, p_full_check);
+
+			uint32_t changed_item_ref_id = h.id();
+
+			params.abb = abb;
+
+			params.result_count_overall = 0; // might not be needed
+			tree.cull_aabb(params, false);
+
+			for (const uint32_t ref_id : tree._cull_hits) {
+				// don't collide against ourself
+				if (ref_id == changed_item_ref_id) {
+					continue;
+				}
+
+				// checkmasks is already done in the cull routine.
+				BVHHandle h_collidee;
+				h_collidee.set_id(ref_id);
+
+				// find NEW enterers, and send callbacks for them only
+				_collide(h, h_collidee);
+			}
+		}
+		_reset();
+	}
+
+public:
+	void item_get_AABB(BVHHandle p_handle, BOUNDS &r_aabb) {
+		DEV_ASSERT(!p_handle.is_invalid());
+		BVHABB_CLASS abb;
+		tree.item_get_ABB(p_handle, abb);
+		abb.to(r_aabb);
+	}
+
+private:
+	// supplemental funcs
+	uint32_t item_get_tree_id(BVHHandle p_handle) const { return _get_extra(p_handle).tree_id; }
+	T *item_get_userdata(BVHHandle p_handle) const { return _get_extra(p_handle).userdata; }
+	int item_get_subindex(BVHHandle p_handle) const { return _get_extra(p_handle).subindex; }
+
+	void _unpair(BVHHandle p_from, BVHHandle p_to) {
+		tree._handle_sort(p_from, p_to);
+
+		typename BVHTREE_CLASS::ItemExtra &exa = tree._extra[p_from.id()];
+		typename BVHTREE_CLASS::ItemExtra &exb = tree._extra[p_to.id()];
+
+		// if the userdata is the same, no collisions should occur
+		if ((exa.userdata == exb.userdata) && exa.userdata) {
+			return;
+		}
+
+		typename BVHTREE_CLASS::ItemPairs &pairs_from = tree._pairs[p_from.id()];
+		typename BVHTREE_CLASS::ItemPairs &pairs_to = tree._pairs[p_to.id()];
+
+		void *ud_from = pairs_from.remove_pair_to(p_to);
+		pairs_to.remove_pair_to(p_from);
+
+#ifdef BVH_VERBOSE_PAIRING
+		print_line("_unpair " + itos(p_from.id()) + " from " + itos(p_to.id()));
+#endif
+
+		// callback
+		if (unpair_callback) {
+			unpair_callback(pair_callback_userdata, p_from, exa.userdata, exa.subindex, p_to, exb.userdata, exb.subindex, ud_from);
+		}
+	}
+
+	void *_recheck_pair(BVHHandle p_from, BVHHandle p_to, void *p_pair_data) {
+		tree._handle_sort(p_from, p_to);
+
+		typename BVHTREE_CLASS::ItemExtra &exa = tree._extra[p_from.id()];
+		typename BVHTREE_CLASS::ItemExtra &exb = tree._extra[p_to.id()];
+
+		// if the userdata is the same, no collisions should occur
+		if ((exa.userdata == exb.userdata) && exa.userdata) {
+			return p_pair_data;
+		}
+
+		// callback
+		if (check_pair_callback) {
+			return check_pair_callback(check_pair_callback_userdata, p_from, exa.userdata, exa.subindex, p_to, exb.userdata, exb.subindex, p_pair_data);
+		}
+
+		return p_pair_data;
+	}
+
+	// returns true if unpair
+	bool _find_leavers_process_pair(typename BVHTREE_CLASS::ItemPairs &p_pairs_from, const BVHABB_CLASS &p_abb_from, BVHHandle p_from, BVHHandle p_to, bool p_full_check) {
+		BVHABB_CLASS abb_to;
+		tree.item_get_ABB(p_to, abb_to);
+
+		// do they overlap?
+		if (p_abb_from.intersects(abb_to)) {
+			// the full check for pairable / non pairable (i.e. tree_id and tree_masks) and mask changes is extra expense
+			// this need not be done in most cases (for speed) except in the case where set_tree is called
+			// where the masks etc of the objects in question may have changed
+			if (!p_full_check) {
+				return false;
+			}
+			const typename BVHTREE_CLASS::ItemExtra &exa = _get_extra(p_from);
+			const typename BVHTREE_CLASS::ItemExtra &exb = _get_extra(p_to);
+
+			// Checking tree_ids and tree_collision_masks
+			if (exa.are_item_trees_compatible(exb)) {
+				bool pair_allowed = USER_PAIR_TEST_FUNCTION::user_pair_check(exa.userdata, exb.userdata);
+
+				// the masks must still be compatible to pair
+				// i.e. if there is a hit between the two and they intersect, then they should stay paired
+				if (pair_allowed) {
+					return false;
+				}
+			}
+		}
+
+		_unpair(p_from, p_to);
+		return true;
+	}
+
+	// find all the existing paired aabbs that are no longer
+	// paired, and send callbacks
+	void _find_leavers(BVHHandle p_handle, const BVHABB_CLASS &expanded_abb_from, bool p_full_check) {
+		typename BVHTREE_CLASS::ItemPairs &p_from = tree._pairs[p_handle.id()];
+
+		BVHABB_CLASS abb_from = expanded_abb_from;
+
+		// remove from pairing list for every partner
+		for (unsigned int n = 0; n < p_from.extended_pairs.size(); n++) {
+			BVHHandle h_to = p_from.extended_pairs[n].handle;
+			if (_find_leavers_process_pair(p_from, abb_from, p_handle, h_to, p_full_check)) {
+				// we need to keep the counter n up to date if we deleted a pair
+				// as the number of items in p_from.extended_pairs will have decreased by 1
+				// and we don't want to miss an item
+				n--;
+			}
+		}
+	}
+
+	// find NEW enterers, and send callbacks for them only
+	// handle a and b
+	void _collide(BVHHandle p_ha, BVHHandle p_hb) {
+		// only have to do this oneway, lower ID then higher ID
+		tree._handle_sort(p_ha, p_hb);
+
+		const typename BVHTREE_CLASS::ItemExtra &exa = _get_extra(p_ha);
+		const typename BVHTREE_CLASS::ItemExtra &exb = _get_extra(p_hb);
+
+		// user collision callback
+		if (!USER_PAIR_TEST_FUNCTION::user_pair_check(exa.userdata, exb.userdata)) {
+			return;
+		}
+
+		// if the userdata is the same, no collisions should occur
+		if ((exa.userdata == exb.userdata) && exa.userdata) {
+			return;
+		}
+
+		typename BVHTREE_CLASS::ItemPairs &p_from = tree._pairs[p_ha.id()];
+		typename BVHTREE_CLASS::ItemPairs &p_to = tree._pairs[p_hb.id()];
+
+		// does this pair exist already?
+		// or only check the one with lower number of pairs for greater speed
+		if (p_from.num_pairs <= p_to.num_pairs) {
+			if (p_from.contains_pair_to(p_hb)) {
+				return;
+			}
+		} else {
+			if (p_to.contains_pair_to(p_ha)) {
+				return;
+			}
+		}
+
+		// callback
+		void *callback_userdata = nullptr;
+
+#ifdef BVH_VERBOSE_PAIRING
+		print_line("_pair " + itos(p_ha.id()) + " to " + itos(p_hb.id()));
+#endif
+
+		if (pair_callback) {
+			callback_userdata = pair_callback(pair_callback_userdata, p_ha, exa.userdata, exa.subindex, p_hb, exb.userdata, exb.subindex);
+		}
+
+		// new pair! .. only really need to store the userdata on the lower handle, but both have storage so...
+		p_from.add_pair_to(p_hb, callback_userdata);
+		p_to.add_pair_to(p_ha, callback_userdata);
+	}
+
+	// if we remove an item, we need to immediately remove the pairs, to prevent reading the pair after deletion
+	void _remove_pairs_containing(BVHHandle p_handle) {
+		typename BVHTREE_CLASS::ItemPairs &p_from = tree._pairs[p_handle.id()];
+
+		// remove from pairing list for every partner.
+		// can't easily use a for loop here, because removing changes the size of the list
+		while (p_from.extended_pairs.size()) {
+			BVHHandle h_to = p_from.extended_pairs[0].handle;
+			_unpair(p_handle, h_to);
+		}
+	}
+
+	// Send pair callbacks again for all existing pairs for the given handle.
+	void _recheck_pairs(BVHHandle p_handle) {
+		typename BVHTREE_CLASS::ItemPairs &from = tree._pairs[p_handle.id()];
+
+		// checking pair for every partner.
+		for (unsigned int n = 0; n < from.extended_pairs.size(); n++) {
+			typename BVHTREE_CLASS::ItemPairs::Link &pair = from.extended_pairs[n];
+			BVHHandle h_to = pair.handle;
+			void *new_pair_data = _recheck_pair(p_handle, h_to, pair.userdata);
+
+			if (new_pair_data != pair.userdata) {
+				pair.userdata = new_pair_data;
+
+				// Update pair data for the second item.
+				typename BVHTREE_CLASS::ItemPairs &to = tree._pairs[h_to.id()];
+				for (unsigned int to_index = 0; to_index < to.extended_pairs.size(); to_index++) {
+					typename BVHTREE_CLASS::ItemPairs::Link &to_pair = to.extended_pairs[to_index];
+					if (to_pair.handle == p_handle) {
+						to_pair.userdata = new_pair_data;
+						break;
+					}
+				}
+			}
+		}
+	}
+
+private:
+	const typename BVHTREE_CLASS::ItemExtra &_get_extra(BVHHandle p_handle) const {
+		return tree._extra[p_handle.id()];
+	}
+	const typename BVHTREE_CLASS::ItemRef &_get_ref(BVHHandle p_handle) const {
+		return tree._refs[p_handle.id()];
+	}
+
+	void _reset() {
+		changed_items.clear();
+		_tick++;
+	}
+
+	void _add_changed_item(BVHHandle p_handle, const BOUNDS &aabb, bool p_check_aabb = true) {
+		// Note that non pairable items can pair with pairable,
+		// so all types must be added to the list
+
+#ifdef BVH_EXPAND_LEAF_AABBS
+		// if using expanded AABB in the leaf, the redundancy check will already have been made
+		BOUNDS &expanded_aabb = tree._pairs[p_handle.id()].expanded_aabb;
+		item_get_AABB(p_handle, expanded_aabb);
+#else
+		// aabb check with expanded aabb. This greatly decreases processing
+		// at the cost of slightly less accurate pairing checks
+		// Note this pairing AABB is separate from the AABB in the actual tree
+		BOUNDS &expanded_aabb = tree._pairs[p_handle.id()].expanded_aabb;
+
+		// passing p_check_aabb false disables the optimization which prevents collision checks if
+		// the aabb hasn't changed. This is needed where set_pairable has been called, but the position
+		// has not changed.
+		if (p_check_aabb && tree.expanded_aabb_encloses_not_shrink(expanded_aabb, aabb)) {
+			return;
+		}
+
+		// ALWAYS update the new expanded aabb, even if already changed once
+		// this tick, because it is vital that the AABB is kept up to date
+		expanded_aabb = aabb;
+		expanded_aabb.grow_by(tree._pairing_expansion);
+#endif
+
+		// this code is to ensure that changed items only appear once on the updated list
+		// collision checking them multiple times is not needed, and repeats the same thing
+		uint32_t &last_updated_tick = tree._extra[p_handle.id()].last_updated_tick;
+
+		if (last_updated_tick == _tick) {
+			return; // already on changed list
+		}
+
+		// mark as on list
+		last_updated_tick = _tick;
+
+		// add to the list
+		changed_items.push_back(p_handle);
+	}
+
+	void _remove_changed_item(BVHHandle p_handle) {
+		// Care has to be taken here for items that are deleted. The ref ID
+		// could be reused on the same tick for new items. This is probably
+		// rare but should be taken into consideration
+
+		// callbacks
+		_remove_pairs_containing(p_handle);
+
+		// remove from changed items (not very efficient yet)
+		for (int n = 0; n < (int)changed_items.size(); n++) {
+			if (changed_items[n] == p_handle) {
+				changed_items.remove_at_unordered(n);
+
+				// because we are using an unordered remove,
+				// the last changed item will now be at spot 'n',
+				// and we need to redo it, so we prevent moving on to
+				// the next n at the next for iteration.
+				n--;
+			}
+		}
+
+		// reset the last updated tick (may not be necessary but just in case)
+		tree._extra[p_handle.id()].last_updated_tick = 0;
+	}
+
+	PairCallback pair_callback = nullptr;
+	UnpairCallback unpair_callback = nullptr;
+	CheckPairCallback check_pair_callback = nullptr;
+	void *pair_callback_userdata = nullptr;
+	void *unpair_callback_userdata = nullptr;
+	void *check_pair_callback_userdata = nullptr;
+
+	BVHTREE_CLASS tree;
+
+	// for collision pairing,
+	// maintain a list of all items moved etc on each frame / tick
+	LocalVector<BVHHandle> changed_items;
+	uint32_t _tick = 1; // Start from 1 so items with 0 indicate never updated.
+
+	class BVHLockedFunction {
+	public:
+		BVHLockedFunction(Mutex *p_mutex, bool p_thread_safe) {
+			// will be compiled out if not set in template
+			if (p_thread_safe) {
+				_mutex = p_mutex;
+				_mutex->lock();
+
+			} else {
+				_mutex = nullptr;
+			}
+		}
+		~BVHLockedFunction() {
+			// will be compiled out if not set in template
+			if (_mutex) {
+				_mutex->unlock();
+			}
+		}
+
+	private:
+		Mutex *_mutex = nullptr;
+	};
+
+	Mutex _mutex;
+
+	// local toggle for turning on and off thread safety in project settings
+	bool _thread_safe = BVH_THREAD_SAFE;
+
+public:
+	BVH_Manager() {}
+};
+
+#undef BVHTREE_CLASS

core/math/bvh_abb.h

@@ -0,0 +1,291 @@
+/**************************************************************************/
+/*  bvh_abb.h                                                             */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "core/math/aabb.h"
+
+// special optimized version of axis aligned bounding box
+template <typename BOUNDS = AABB, typename POINT = Vector3>
+struct BVH_ABB {
+	struct ConvexHull {
+		// convex hulls (optional)
+		const Plane *planes;
+		int num_planes;
+		const Vector3 *points;
+		int num_points;
+	};
+
+	struct Segment {
+		POINT from;
+		POINT to;
+	};
+
+	enum IntersectResult {
+		IR_MISS = 0,
+		IR_PARTIAL,
+		IR_FULL,
+	};
+
+	// we store mins with a negative value in order to test them with SIMD
+	POINT min;
+	POINT neg_max;
+
+	bool operator==(const BVH_ABB &o) const { return (min == o.min) && (neg_max == o.neg_max); }
+	bool operator!=(const BVH_ABB &o) const { return (*this == o) == false; }
+
+	void set(const POINT &_min, const POINT &_max) {
+		min = _min;
+		neg_max = -_max;
+	}
+
+	// to and from standard AABB
+	void from(const BOUNDS &p_aabb) {
+		min = p_aabb.position;
+		neg_max = -(p_aabb.position + p_aabb.size);
+	}
+
+	void to(BOUNDS &r_aabb) const {
+		r_aabb.position = min;
+		r_aabb.size = calculate_size();
+	}
+
+	void merge(const BVH_ABB &p_o) {
+		for (int axis = 0; axis < POINT::AXIS_COUNT; ++axis) {
+			neg_max[axis] = MIN(neg_max[axis], p_o.neg_max[axis]);
+			min[axis] = MIN(min[axis], p_o.min[axis]);
+		}
+	}
+
+	POINT calculate_size() const {
+		return -neg_max - min;
+	}
+
+	POINT calculate_center() const {
+		return POINT((calculate_size() * 0.5) + min);
+	}
+
+	real_t get_proximity_to(const BVH_ABB &p_b) const {
+		const POINT d = (min - neg_max) - (p_b.min - p_b.neg_max);
+		real_t proximity = 0.0;
+		for (int axis = 0; axis < POINT::AXIS_COUNT; ++axis) {
+			proximity += Math::abs(d[axis]);
+		}
+		return proximity;
+	}
+
+	int select_by_proximity(const BVH_ABB &p_a, const BVH_ABB &p_b) const {
+		return (get_proximity_to(p_a) < get_proximity_to(p_b) ? 0 : 1);
+	}
+
+	uint32_t find_cutting_planes(const typename BVH_ABB::ConvexHull &p_hull, uint32_t *p_plane_ids) const {
+		uint32_t count = 0;
+
+		for (int n = 0; n < p_hull.num_planes; n++) {
+			const Plane &p = p_hull.planes[n];
+			if (intersects_plane(p)) {
+				p_plane_ids[count++] = n;
+			}
+		}
+
+		return count;
+	}
+
+	bool intersects_plane(const Plane &p_p) const {
+		Vector3 size = calculate_size();
+		Vector3 half_extents = size * 0.5;
+		Vector3 ofs = min + half_extents;
+
+		// forward side of plane?
+		Vector3 point_offset(
+				(p_p.normal.x < 0) ? -half_extents.x : half_extents.x,
+				(p_p.normal.y < 0) ? -half_extents.y : half_extents.y,
+				(p_p.normal.z < 0) ? -half_extents.z : half_extents.z);
+		Vector3 point = point_offset + ofs;
+
+		if (!p_p.is_point_over(point)) {
+			return false;
+		}
+
+		point = -point_offset + ofs;
+		if (p_p.is_point_over(point)) {
+			return false;
+		}
+
+		return true;
+	}
+
+	bool intersects_convex_optimized(const ConvexHull &p_hull, const uint32_t *p_plane_ids, uint32_t p_num_planes) const {
+		Vector3 size = calculate_size();
+		Vector3 half_extents = size * 0.5;
+		Vector3 ofs = min + half_extents;
+
+		for (unsigned int i = 0; i < p_num_planes; i++) {
+			const Plane &p = p_hull.planes[p_plane_ids[i]];
+			Vector3 point(
+					(p.normal.x > 0) ? -half_extents.x : half_extents.x,
+					(p.normal.y > 0) ? -half_extents.y : half_extents.y,
+					(p.normal.z > 0) ? -half_extents.z : half_extents.z);
+			point += ofs;
+			if (p.is_point_over(point)) {
+				return false;
+			}
+		}
+
+		return true;
+	}
+
+	bool intersects_convex_partial(const ConvexHull &p_hull) const {
+		BOUNDS bb;
+		to(bb);
+		return bb.intersects_convex_shape(p_hull.planes, p_hull.num_planes, p_hull.points, p_hull.num_points);
+	}
+
+	IntersectResult intersects_convex(const ConvexHull &p_hull) const {
+		if (intersects_convex_partial(p_hull)) {
+			// fully within? very important for tree checks
+			if (is_within_convex(p_hull)) {
+				return IR_FULL;
+			}
+
+			return IR_PARTIAL;
+		}
+
+		return IR_MISS;
+	}
+
+	bool is_within_convex(const ConvexHull &p_hull) const {
+		// use half extents routine
+		BOUNDS bb;
+		to(bb);
+		return bb.inside_convex_shape(p_hull.planes, p_hull.num_planes);
+	}
+
+	bool is_point_within_hull(const ConvexHull &p_hull, const Vector3 &p_pt) const {
+		for (int n = 0; n < p_hull.num_planes; n++) {
+			if (p_hull.planes[n].distance_to(p_pt) > 0.0f) {
+				return false;
+			}
+		}
+		return true;
+	}
+
+	bool intersects_segment(const Segment &p_s) const {
+		BOUNDS bb;
+		to(bb);
+		return bb.intersects_segment(p_s.from, p_s.to);
+	}
+
+	bool intersects_point(const POINT &p_pt) const {
+		if (_any_lessthan(-p_pt, neg_max)) {
+			return false;
+		}
+		if (_any_lessthan(p_pt, min)) {
+			return false;
+		}
+		return true;
+	}
+
+	// Very hot in profiling, make sure optimized
+	bool intersects(const BVH_ABB &p_o) const {
+		if (_any_morethan(p_o.min, -neg_max)) {
+			return false;
+		}
+		if (_any_morethan(min, -p_o.neg_max)) {
+			return false;
+		}
+		return true;
+	}
+
+	// for pre-swizzled tester (this object)
+	bool intersects_swizzled(const BVH_ABB &p_o) const {
+		if (_any_lessthan(min, p_o.min)) {
+			return false;
+		}
+		if (_any_lessthan(neg_max, p_o.neg_max)) {
+			return false;
+		}
+		return true;
+	}
+
+	bool is_other_within(const BVH_ABB &p_o) const {
+		if (_any_lessthan(p_o.neg_max, neg_max)) {
+			return false;
+		}
+		if (_any_lessthan(p_o.min, min)) {
+			return false;
+		}
+		return true;
+	}
+
+	void grow(const POINT &p_change) {
+		neg_max -= p_change;
+		min -= p_change;
+	}
+
+	void expand(real_t p_change) {
+		POINT change;
+		for (int axis = 0; axis < POINT::AXIS_COUNT; ++axis) {
+			change[axis] = p_change;
+		}
+		grow(change);
+	}
+
+	// Actually surface area metric.
+	real_t get_area() const {
+		POINT d = calculate_size();
+		return 2.0f * (d.x * d.y + d.y * d.z + d.z * d.x);
+	}
+
+	void set_to_max_opposite_extents() {
+		for (int axis = 0; axis < POINT::AXIS_COUNT; ++axis) {
+			neg_max[axis] = FLT_MAX;
+		}
+		min = neg_max;
+	}
+
+	bool _any_morethan(const POINT &p_a, const POINT &p_b) const {
+		for (int axis = 0; axis < POINT::AXIS_COUNT; ++axis) {
+			if (p_a[axis] > p_b[axis]) {
+				return true;
+			}
+		}
+		return false;
+	}
+
+	bool _any_lessthan(const POINT &p_a, const POINT &p_b) const {
+		for (int axis = 0; axis < POINT::AXIS_COUNT; ++axis) {
+			if (p_a[axis] < p_b[axis]) {
+				return true;
+			}
+		}
+		return false;
+	}
+};

core/math/bvh_cull.inc

@@ -0,0 +1,574 @@
+public:
+// cull parameters is a convenient way of passing a bunch
+// of arguments through the culling functions without
+// writing loads of code. Not all members are used for some cull checks
+struct CullParams {
+	int result_count_overall; // both trees
+	int result_count; // this tree only
+	int result_max;
+	T **result_array;
+	int *subindex_array;
+
+	// We now process masks etc in a user template function,
+	// and these for simplicity assume even for cull tests there is a
+	// testing object (which has masks etc) for the user cull checks.
+	// This means for cull tests on their own, the client will usually
+	// want to create a dummy object, just in order to specify masks etc.
+	const T *tester;
+
+	// optional components for different tests
+	POINT point;
+	BVHABB_CLASS abb;
+	typename BVHABB_CLASS::ConvexHull hull;
+	typename BVHABB_CLASS::Segment segment;
+
+	// When collision testing, we can specify which tree ids
+	// to collide test against with the tree_collision_mask.
+	uint32_t tree_collision_mask;
+};
+
+private:
+void _cull_translate_hits(CullParams &p) {
+	int num_hits = _cull_hits.size();
+	int left = p.result_max - p.result_count_overall;
+
+	if (num_hits > left) {
+		num_hits = left;
+	}
+
+	int out_n = p.result_count_overall;
+
+	for (int n = 0; n < num_hits; n++) {
+		uint32_t ref_id = _cull_hits[n];
+
+		const ItemExtra &ex = _extra[ref_id];
+		p.result_array[out_n] = ex.userdata;
+
+		if (p.subindex_array) {
+			p.subindex_array[out_n] = ex.subindex;
+		}
+
+		out_n++;
+	}
+
+	p.result_count = num_hits;
+	p.result_count_overall += num_hits;
+}
+
+public:
+int cull_convex(CullParams &r_params, bool p_translate_hits = true) {
+	_cull_hits.clear();
+	r_params.result_count = 0;
+
+	uint32_t tree_test_mask = 0;
+
+	for (int n = 0; n < NUM_TREES; n++) {
+		tree_test_mask <<= 1;
+		if (!tree_test_mask) {
+			tree_test_mask = 1;
+		}
+
+		if (_root_node_id[n] == BVHCommon::INVALID) {
+			continue;
+		}
+
+		if (!(r_params.tree_collision_mask & tree_test_mask)) {
+			continue;
+		}
+
+		_cull_convex_iterative(_root_node_id[n], r_params);
+	}
+
+	if (p_translate_hits) {
+		_cull_translate_hits(r_params);
+	}
+
+	return r_params.result_count;
+}
+
+int cull_segment(CullParams &r_params, bool p_translate_hits = true) {
+	_cull_hits.clear();
+	r_params.result_count = 0;
+
+	uint32_t tree_test_mask = 0;
+
+	for (int n = 0; n < NUM_TREES; n++) {
+		tree_test_mask <<= 1;
+		if (!tree_test_mask) {
+			tree_test_mask = 1;
+		}
+
+		if (_root_node_id[n] == BVHCommon::INVALID) {
+			continue;
+		}
+
+		if (!(r_params.tree_collision_mask & tree_test_mask)) {
+			continue;
+		}
+
+		_cull_segment_iterative(_root_node_id[n], r_params);
+	}
+
+	if (p_translate_hits) {
+		_cull_translate_hits(r_params);
+	}
+
+	return r_params.result_count;
+}
+
+int cull_point(CullParams &r_params, bool p_translate_hits = true) {
+	_cull_hits.clear();
+	r_params.result_count = 0;
+
+	uint32_t tree_test_mask = 0;
+
+	for (int n = 0; n < NUM_TREES; n++) {
+		tree_test_mask <<= 1;
+		if (!tree_test_mask) {
+			tree_test_mask = 1;
+		}
+
+		if (_root_node_id[n] == BVHCommon::INVALID) {
+			continue;
+		}
+
+		if (!(r_params.tree_collision_mask & tree_test_mask)) {
+			continue;
+		}
+
+		_cull_point_iterative(_root_node_id[n], r_params);
+	}
+
+	if (p_translate_hits) {
+		_cull_translate_hits(r_params);
+	}
+
+	return r_params.result_count;
+}
+
+int cull_aabb(CullParams &r_params, bool p_translate_hits = true) {
+	_cull_hits.clear();
+	r_params.result_count = 0;
+
+	uint32_t tree_test_mask = 0;
+
+	for (int n = 0; n < NUM_TREES; n++) {
+		tree_test_mask <<= 1;
+		if (!tree_test_mask) {
+			tree_test_mask = 1;
+		}
+
+		if (_root_node_id[n] == BVHCommon::INVALID) {
+			continue;
+		}
+
+		// the tree collision mask determines which trees to collide test against
+		if (!(r_params.tree_collision_mask & tree_test_mask)) {
+			continue;
+		}
+
+		_cull_aabb_iterative(_root_node_id[n], r_params);
+	}
+
+	if (p_translate_hits) {
+		_cull_translate_hits(r_params);
+	}
+
+	return r_params.result_count;
+}
+
+bool _cull_hits_full(const CullParams &p) {
+	// instead of checking every hit, we can do a lazy check for this condition.
+	// it isn't a problem if we write too much _cull_hits because they only the
+	// result_max amount will be translated and outputted. But we might as
+	// well stop our cull checks after the maximum has been reached.
+	return (int)_cull_hits.size() >= p.result_max;
+}
+
+void _cull_hit(uint32_t p_ref_id, CullParams &p) {
+	// take into account masks etc
+	// this would be more efficient to do before plane checks,
+	// but done here for ease to get started
+	if (USE_PAIRS) {
+		const ItemExtra &ex = _extra[p_ref_id];
+
+		// user supplied function (for e.g. pairable types and pairable masks in the render tree)
+		if (!USER_CULL_TEST_FUNCTION::user_cull_check(p.tester, ex.userdata)) {
+			return;
+		}
+	}
+
+	_cull_hits.push_back(p_ref_id);
+}
+
+bool _cull_segment_iterative(uint32_t p_node_id, CullParams &r_params) {
+	// our function parameters to keep on a stack
+	struct CullSegParams {
+		uint32_t node_id;
+	};
+
+	// most of the iterative functionality is contained in this helper class
+	BVH_IterativeInfo<CullSegParams> ii;
+
+	// alloca must allocate the stack from this function, it cannot be allocated in the
+	// helper class
+	ii.stack = (CullSegParams *)alloca(ii.get_alloca_stacksize());
+
+	// seed the stack
+	ii.get_first()->node_id = p_node_id;
+
+	CullSegParams csp;
+
+	// while there are still more nodes on the stack
+	while (ii.pop(csp)) {
+		TNode &tnode = _nodes[csp.node_id];
+
+		if (tnode.is_leaf()) {
+			// lazy check for hits full up condition
+			if (_cull_hits_full(r_params)) {
+				return false;
+			}
+
+			TLeaf &leaf = _node_get_leaf(tnode);
+
+			// test children individually
+			for (int n = 0; n < leaf.num_items; n++) {
+				const BVHABB_CLASS &aabb = leaf.get_aabb(n);
+
+				if (aabb.intersects_segment(r_params.segment)) {
+					uint32_t child_id = leaf.get_item_ref_id(n);
+
+					// register hit
+					_cull_hit(child_id, r_params);
+				}
+			}
+		} else {
+			// test children individually
+			for (int n = 0; n < tnode.num_children; n++) {
+				uint32_t child_id = tnode.children[n];
+				const BVHABB_CLASS &child_abb = _nodes[child_id].aabb;
+
+				if (child_abb.intersects_segment(r_params.segment)) {
+					// add to the stack
+					CullSegParams *child = ii.request();
+					child->node_id = child_id;
+				}
+			}
+		}
+
+	} // while more nodes to pop
+
+	// true indicates results are not full
+	return true;
+}
+
+bool _cull_point_iterative(uint32_t p_node_id, CullParams &r_params) {
+	// our function parameters to keep on a stack
+	struct CullPointParams {
+		uint32_t node_id;
+	};
+
+	// most of the iterative functionality is contained in this helper class
+	BVH_IterativeInfo<CullPointParams> ii;
+
+	// alloca must allocate the stack from this function, it cannot be allocated in the
+	// helper class
+	ii.stack = (CullPointParams *)alloca(ii.get_alloca_stacksize());
+
+	// seed the stack
+	ii.get_first()->node_id = p_node_id;
+
+	CullPointParams cpp;
+
+	// while there are still more nodes on the stack
+	while (ii.pop(cpp)) {
+		TNode &tnode = _nodes[cpp.node_id];
+		// no hit with this node?
+		if (!tnode.aabb.intersects_point(r_params.point)) {
+			continue;
+		}
+
+		if (tnode.is_leaf()) {
+			// lazy check for hits full up condition
+			if (_cull_hits_full(r_params)) {
+				return false;
+			}
+
+			TLeaf &leaf = _node_get_leaf(tnode);
+
+			// test children individually
+			for (int n = 0; n < leaf.num_items; n++) {
+				if (leaf.get_aabb(n).intersects_point(r_params.point)) {
+					uint32_t child_id = leaf.get_item_ref_id(n);
+
+					// register hit
+					_cull_hit(child_id, r_params);
+				}
+			}
+		} else {
+			// test children individually
+			for (int n = 0; n < tnode.num_children; n++) {
+				uint32_t child_id = tnode.children[n];
+
+				// add to the stack
+				CullPointParams *child = ii.request();
+				child->node_id = child_id;
+			}
+		}
+
+	} // while more nodes to pop
+
+	// true indicates results are not full
+	return true;
+}
+
+// Note: This is a very hot loop profiling wise. Take care when changing this and profile.
+bool _cull_aabb_iterative(uint32_t p_node_id, CullParams &r_params, bool p_fully_within = false) {
+	// our function parameters to keep on a stack
+	struct CullAABBParams {
+		uint32_t node_id;
+		bool fully_within;
+	};
+
+	// most of the iterative functionality is contained in this helper class
+	BVH_IterativeInfo<CullAABBParams> ii;
+
+	// alloca must allocate the stack from this function, it cannot be allocated in the
+	// helper class
+	ii.stack = (CullAABBParams *)alloca(ii.get_alloca_stacksize());
+
+	// seed the stack
+	ii.get_first()->node_id = p_node_id;
+	ii.get_first()->fully_within = p_fully_within;
+
+	CullAABBParams cap;
+
+	// while there are still more nodes on the stack
+	while (ii.pop(cap)) {
+		TNode &tnode = _nodes[cap.node_id];
+
+		if (tnode.is_leaf()) {
+			// lazy check for hits full up condition
+			if (_cull_hits_full(r_params)) {
+				return false;
+			}
+
+			TLeaf &leaf = _node_get_leaf(tnode);
+
+			// if fully within we can just add all items
+			// as long as they pass mask checks
+			if (cap.fully_within) {
+				for (int n = 0; n < leaf.num_items; n++) {
+					uint32_t child_id = leaf.get_item_ref_id(n);
+
+					// register hit
+					_cull_hit(child_id, r_params);
+				}
+			} else {
+				// This section is the hottest area in profiling, so
+				// is optimized highly
+				// get this into a local register and preconverted to correct type
+				int leaf_num_items = leaf.num_items;
+
+				BVHABB_CLASS swizzled_tester;
+				swizzled_tester.min = -r_params.abb.neg_max;
+				swizzled_tester.neg_max = -r_params.abb.min;
+
+				for (int n = 0; n < leaf_num_items; n++) {
+					const BVHABB_CLASS &aabb = leaf.get_aabb(n);
+
+					if (swizzled_tester.intersects_swizzled(aabb)) {
+						uint32_t child_id = leaf.get_item_ref_id(n);
+
+						// register hit
+						_cull_hit(child_id, r_params);
+					}
+				}
+
+			} // not fully within
+		} else {
+			if (!cap.fully_within) {
+				// test children individually
+				for (int n = 0; n < tnode.num_children; n++) {
+					uint32_t child_id = tnode.children[n];
+					const BVHABB_CLASS &child_abb = _nodes[child_id].aabb;
+
+					if (child_abb.intersects(r_params.abb)) {
+						// is the node totally within the aabb?
+						bool fully_within = r_params.abb.is_other_within(child_abb);
+
+						// add to the stack
+						CullAABBParams *child = ii.request();
+
+						// should always return valid child
+						child->node_id = child_id;
+						child->fully_within = fully_within;
+					}
+				}
+			} else {
+				for (int n = 0; n < tnode.num_children; n++) {
+					uint32_t child_id = tnode.children[n];
+
+					// add to the stack
+					CullAABBParams *child = ii.request();
+
+					// should always return valid child
+					child->node_id = child_id;
+					child->fully_within = true;
+				}
+			}
+		}
+
+	} // while more nodes to pop
+
+	// true indicates results are not full
+	return true;
+}
+
+// returns full up with results
+bool _cull_convex_iterative(uint32_t p_node_id, CullParams &r_params, bool p_fully_within = false) {
+	// our function parameters to keep on a stack
+	struct CullConvexParams {
+		uint32_t node_id;
+		bool fully_within;
+	};
+
+	// most of the iterative functionality is contained in this helper class
+	BVH_IterativeInfo<CullConvexParams> ii;
+
+	// alloca must allocate the stack from this function, it cannot be allocated in the
+	// helper class
+	ii.stack = (CullConvexParams *)alloca(ii.get_alloca_stacksize());
+
+	// seed the stack
+	ii.get_first()->node_id = p_node_id;
+	ii.get_first()->fully_within = p_fully_within;
+
+	// preallocate these as a once off to be reused
+	uint32_t max_planes = r_params.hull.num_planes;
+	uint32_t *plane_ids = (uint32_t *)alloca(sizeof(uint32_t) * max_planes);
+
+	CullConvexParams ccp;
+
+	// while there are still more nodes on the stack
+	while (ii.pop(ccp)) {
+		const TNode &tnode = _nodes[ccp.node_id];
+
+		if (!ccp.fully_within) {
+			typename BVHABB_CLASS::IntersectResult res = tnode.aabb.intersects_convex(r_params.hull);
+
+			switch (res) {
+				default: {
+					continue; // miss, just move on to the next node in the stack
+				} break;
+				case BVHABB_CLASS::IR_PARTIAL: {
+				} break;
+				case BVHABB_CLASS::IR_FULL: {
+					ccp.fully_within = true;
+				} break;
+			}
+
+		} // if not fully within already
+
+		if (tnode.is_leaf()) {
+			// lazy check for hits full up condition
+			if (_cull_hits_full(r_params)) {
+				return false;
+			}
+
+			const TLeaf &leaf = _node_get_leaf(tnode);
+
+			// if fully within, simply add all items to the result
+			// (taking into account masks)
+			if (ccp.fully_within) {
+				for (int n = 0; n < leaf.num_items; n++) {
+					uint32_t child_id = leaf.get_item_ref_id(n);
+
+					// register hit
+					_cull_hit(child_id, r_params);
+				}
+
+			} else {
+				// we can either use a naive check of all the planes against the AABB,
+				// or an optimized check, which finds in advance which of the planes can possibly
+				// cut the AABB, and only tests those. This can be much faster.
+#define BVH_CONVEX_CULL_OPTIMIZED
+#ifdef BVH_CONVEX_CULL_OPTIMIZED
+				// first find which planes cut the aabb
+				uint32_t num_planes = tnode.aabb.find_cutting_planes(r_params.hull, plane_ids);
+				BVH_ASSERT(num_planes <= max_planes);
+
+//#define BVH_CONVEX_CULL_OPTIMIZED_RIGOR_CHECK
+#ifdef BVH_CONVEX_CULL_OPTIMIZED_RIGOR_CHECK
+				// rigorous check
+				uint32_t results[MAX_ITEMS];
+				uint32_t num_results = 0;
+#endif
+
+				// test children individually
+				for (int n = 0; n < leaf.num_items; n++) {
+					//const Item &item = leaf.get_item(n);
+					const BVHABB_CLASS &aabb = leaf.get_aabb(n);
+
+					if (aabb.intersects_convex_optimized(r_params.hull, plane_ids, num_planes)) {
+						uint32_t child_id = leaf.get_item_ref_id(n);
+
+#ifdef BVH_CONVEX_CULL_OPTIMIZED_RIGOR_CHECK
+						results[num_results++] = child_id;
+#endif
+
+						// register hit
+						_cull_hit(child_id, r_params);
+					}
+				}
+
+#ifdef BVH_CONVEX_CULL_OPTIMIZED_RIGOR_CHECK
+				uint32_t test_count = 0;
+
+				for (int n = 0; n < leaf.num_items; n++) {
+					const BVHABB_CLASS &aabb = leaf.get_aabb(n);
+
+					if (aabb.intersects_convex_partial(r_params.hull)) {
+						uint32_t child_id = leaf.get_item_ref_id(n);
+
+						CRASH_COND(child_id != results[test_count++]);
+						CRASH_COND(test_count > num_results);
+					}
+				}
+#endif
+
+#else
+				// not BVH_CONVEX_CULL_OPTIMIZED
+				// test children individually
+				for (int n = 0; n < leaf.num_items; n++) {
+					const BVHABB_CLASS &aabb = leaf.get_aabb(n);
+
+					if (aabb.intersects_convex_partial(r_params.hull)) {
+						uint32_t child_id = leaf.get_item_ref_id(n);
+
+						// full up with results? exit early, no point in further testing
+						if (!_cull_hit(child_id, r_params)) {
+							return false;
+						}
+					}
+				}
+#endif // BVH_CONVEX_CULL_OPTIMIZED
+			} // if not fully within
+		} else {
+			for (int n = 0; n < tnode.num_children; n++) {
+				uint32_t child_id = tnode.children[n];
+
+				// add to the stack
+				CullConvexParams *child = ii.request();
+
+				// should always return valid child
+				child->node_id = child_id;
+				child->fully_within = ccp.fully_within;
+			}
+		}
+
+	} // while more nodes to pop
+
+	// true indicates results are not full
+	return true;
+}

core/math/bvh_debug.inc

@@ -0,0 +1,63 @@
+public:
+#ifdef BVH_VERBOSE
+void _debug_recursive_print_tree(int p_tree_id) const {
+	if (_root_node_id[p_tree_id] != BVHCommon::INVALID) {
+		_debug_recursive_print_tree_node(_root_node_id[p_tree_id]);
+	}
+}
+
+String _debug_aabb_to_string(const BVHABB_CLASS &aabb) const {
+	POINT size = aabb.calculate_size();
+
+	String sz;
+	real_t vol = 0.0;
+
+	for (int i = 0; i < POINT::AXIS_COUNT; ++i) {
+		sz += "(";
+		sz += itos(aabb.min[i]);
+		sz += " ~ ";
+		sz += itos(-aabb.neg_max[i]);
+		sz += ") ";
+
+		vol += size[i];
+	}
+
+	sz += "vol " + itos(vol);
+
+	return sz;
+}
+
+void _debug_recursive_print_tree_node(uint32_t p_node_id, int depth = 0) const {
+	const TNode &tnode = _nodes[p_node_id];
+
+	String sz = String("\t").repeat(depth) + itos(p_node_id);
+
+	if (tnode.is_leaf()) {
+		sz += " L";
+		sz += itos(tnode.height) + " ";
+		const TLeaf &leaf = _node_get_leaf(tnode);
+
+		sz += "[";
+		for (int n = 0; n < leaf.num_items; n++) {
+			if (n) {
+				sz += ", ";
+			}
+			sz += "r";
+			sz += itos(leaf.get_item_ref_id(n));
+		}
+		sz += "]  ";
+	} else {
+		sz += " N";
+		sz += itos(tnode.height) + " ";
+	}
+
+	sz += _debug_aabb_to_string(tnode.aabb);
+	print_line(sz);
+
+	if (!tnode.is_leaf()) {
+		for (int n = 0; n < tnode.num_children; n++) {
+			_debug_recursive_print_tree_node(tnode.children[n], depth + 1);
+		}
+	}
+}
+#endif

core/math/bvh_integrity.inc

@@ -0,0 +1,42 @@
+void _integrity_check_all() {
+#ifdef BVH_INTEGRITY_CHECKS
+	for (int n = 0; n < NUM_TREES; n++) {
+		uint32_t root = _root_node_id[n];
+		if (root != BVHCommon::INVALID) {
+			_integrity_check_down(root);
+		}
+	}
+#endif
+}
+
+void _integrity_check_up(uint32_t p_node_id) {
+	TNode &node = _nodes[p_node_id];
+
+	BVHABB_CLASS abb = node.aabb;
+	node_update_aabb(node);
+
+	BVHABB_CLASS abb2 = node.aabb;
+	abb2.expand(-_node_expansion);
+
+	CRASH_COND(!abb.is_other_within(abb2));
+}
+
+void _integrity_check_down(uint32_t p_node_id) {
+	const TNode &node = _nodes[p_node_id];
+
+	if (node.is_leaf()) {
+		_integrity_check_up(p_node_id);
+	} else {
+		CRASH_COND(node.num_children != 2);
+
+		for (int n = 0; n < node.num_children; n++) {
+			uint32_t child_id = node.children[n];
+
+			// check the children parent pointers are correct
+			TNode &child = _nodes[child_id];
+			CRASH_COND(child.parent_id != p_node_id);
+
+			_integrity_check_down(child_id);
+		}
+	}
+}

core/math/bvh_logic.inc

@@ -0,0 +1,229 @@
+// for slow incremental optimization, we will periodically remove each
+// item from the tree and reinsert, to give it a chance to find a better position
+void _logic_item_remove_and_reinsert(uint32_t p_ref_id) {
+	// get the reference
+	ItemRef &ref = _refs[p_ref_id];
+
+	// no need to optimize inactive items
+	if (!ref.is_active()) {
+		return;
+	}
+
+	// special case of debug draw
+	if (ref.item_id == BVHCommon::INVALID) {
+		return;
+	}
+
+	BVH_ASSERT(ref.tnode_id != BVHCommon::INVALID);
+
+	// some overlay elaborate way to find out which tree the node is in!
+	BVHHandle temp_handle;
+	temp_handle.set_id(p_ref_id);
+	uint32_t tree_id = _handle_get_tree_id(temp_handle);
+
+	// remove and reinsert
+	BVHABB_CLASS abb;
+	node_remove_item(p_ref_id, tree_id, &abb);
+
+	// we must choose where to add to tree
+	ref.tnode_id = _logic_choose_item_add_node(_root_node_id[tree_id], abb);
+	_node_add_item(ref.tnode_id, p_ref_id, abb);
+
+	refit_upward_and_balance(ref.tnode_id, tree_id);
+}
+
+// from randy gaul balance function
+BVHABB_CLASS _logic_abb_merge(const BVHABB_CLASS &a, const BVHABB_CLASS &b) {
+	BVHABB_CLASS c = a;
+	c.merge(b);
+	return c;
+}
+
+//--------------------------------------------------------------------------------------------------
+/**
+ * @file    q3DynamicAABBTree.h
+ * @author  Randy Gaul
+ * @date    10/10/2014
+ *  Copyright (c) 2014 Randy Gaul http://www.randygaul.net
+ *  This software is provided 'as-is', without any express or implied
+ *  warranty. In no event will the authors be held liable for any damages
+ *  arising from the use of this software.
+ *  Permission is granted to anyone to use this software for any purpose,
+ *  including commercial applications, and to alter it and redistribute it
+ *  freely, subject to the following restrictions:
+ *    1. The origin of this software must not be misrepresented; you must not
+ *       claim that you wrote the original software. If you use this software
+ *       in a product, an acknowledgment in the product documentation would be
+ *       appreciated but is not required.
+ *    2. Altered source versions must be plainly marked as such, and must not
+ *       be misrepresented as being the original software.
+ *    3. This notice may not be removed or altered from any source distribution.
+ */
+//--------------------------------------------------------------------------------------------------
+
+// This function is based on the 'Balance' function from Randy Gaul's qu3e
+// https://github.com/RandyGaul/qu3e
+// It is MODIFIED from qu3e version.
+// This is the only function used (and _logic_abb_merge helper function).
+int32_t _logic_balance(int32_t iA, uint32_t p_tree_id) {
+	//return iA; // uncomment this to bypass balance
+
+	TNode *A = &_nodes[iA];
+
+	if (A->is_leaf() || A->height == 1) {
+		return iA;
+	}
+
+	/*        A
+	 *      /   \
+	 *     B     C
+	 *    / \   / \
+	 *   D   E F   G
+	 */
+
+	CRASH_COND(A->num_children != 2);
+	int32_t iB = A->children[0];
+	int32_t iC = A->children[1];
+	TNode *B = &_nodes[iB];
+	TNode *C = &_nodes[iC];
+
+	int32_t balance = C->height - B->height;
+
+	// C is higher, promote C
+	if (balance > 1) {
+		int32_t iF = C->children[0];
+		int32_t iG = C->children[1];
+		TNode *F = &_nodes[iF];
+		TNode *G = &_nodes[iG];
+
+		// grandParent point to C
+		if (A->parent_id != BVHCommon::INVALID) {
+			if (_nodes[A->parent_id].children[0] == iA) {
+				_nodes[A->parent_id].children[0] = iC;
+
+			} else {
+				_nodes[A->parent_id].children[1] = iC;
+			}
+		} else {
+			// check this .. seems dodgy
+			change_root_node(iC, p_tree_id);
+		}
+
+		// Swap A and C
+		C->children[0] = iA;
+		C->parent_id = A->parent_id;
+		A->parent_id = iC;
+
+		// Finish rotation
+		if (F->height > G->height) {
+			C->children[1] = iF;
+			A->children[1] = iG;
+			G->parent_id = iA;
+			A->aabb = _logic_abb_merge(B->aabb, G->aabb);
+			C->aabb = _logic_abb_merge(A->aabb, F->aabb);
+
+			A->height = 1 + MAX(B->height, G->height);
+			C->height = 1 + MAX(A->height, F->height);
+		}
+
+		else {
+			C->children[1] = iG;
+			A->children[1] = iF;
+			F->parent_id = iA;
+			A->aabb = _logic_abb_merge(B->aabb, F->aabb);
+			C->aabb = _logic_abb_merge(A->aabb, G->aabb);
+
+			A->height = 1 + MAX(B->height, F->height);
+			C->height = 1 + MAX(A->height, G->height);
+		}
+
+		return iC;
+	}
+
+	// B is higher, promote B
+	else if (balance < -1) {
+		int32_t iD = B->children[0];
+		int32_t iE = B->children[1];
+		TNode *D = &_nodes[iD];
+		TNode *E = &_nodes[iE];
+
+		// grandParent point to B
+		if (A->parent_id != BVHCommon::INVALID) {
+			if (_nodes[A->parent_id].children[0] == iA) {
+				_nodes[A->parent_id].children[0] = iB;
+			} else {
+				_nodes[A->parent_id].children[1] = iB;
+			}
+		}
+
+		else {
+			// check this .. seems dodgy
+			change_root_node(iB, p_tree_id);
+		}
+
+		// Swap A and B
+		B->children[1] = iA;
+		B->parent_id = A->parent_id;
+		A->parent_id = iB;
+
+		// Finish rotation
+		if (D->height > E->height) {
+			B->children[0] = iD;
+			A->children[0] = iE;
+			E->parent_id = iA;
+			A->aabb = _logic_abb_merge(C->aabb, E->aabb);
+			B->aabb = _logic_abb_merge(A->aabb, D->aabb);
+
+			A->height = 1 + MAX(C->height, E->height);
+			B->height = 1 + MAX(A->height, D->height);
+		}
+
+		else {
+			B->children[0] = iE;
+			A->children[0] = iD;
+			D->parent_id = iA;
+			A->aabb = _logic_abb_merge(C->aabb, D->aabb);
+			B->aabb = _logic_abb_merge(A->aabb, E->aabb);
+
+			A->height = 1 + MAX(C->height, D->height);
+			B->height = 1 + MAX(A->height, E->height);
+		}
+
+		return iB;
+	}
+
+	return iA;
+}
+
+// either choose an existing node to add item to, or create a new node and return this
+uint32_t _logic_choose_item_add_node(uint32_t p_node_id, const BVHABB_CLASS &p_aabb) {
+	while (true) {
+		BVH_ASSERT(p_node_id != BVHCommon::INVALID);
+		TNode &tnode = _nodes[p_node_id];
+
+		if (tnode.is_leaf()) {
+			// if a leaf, and non full, use this to add to
+			if (!node_is_leaf_full(tnode)) {
+				return p_node_id;
+			}
+
+			// else split the leaf, and use one of the children to add to
+			return split_leaf(p_node_id, p_aabb);
+		}
+
+		// this should not happen???
+		// is still happening, need to debug and find circumstances. Is not that serious
+		// but would be nice to prevent. I think it only happens with the root node.
+		if (tnode.num_children == 1) {
+			WARN_PRINT_ONCE("BVH::recursive_choose_item_add_node, node with 1 child, recovering");
+			p_node_id = tnode.children[0];
+		} else {
+			BVH_ASSERT(tnode.num_children == 2);
+			TNode &childA = _nodes[tnode.children[0]];
+			TNode &childB = _nodes[tnode.children[1]];
+			int which = p_aabb.select_by_proximity(childA.aabb, childB.aabb);
+
+			p_node_id = tnode.children[which];
+		}
+	}
+}

core/math/bvh_misc.inc

@@ -0,0 +1,50 @@
+int _handle_get_tree_id(BVHHandle p_handle) const {
+	if (USE_PAIRS) {
+		return _extra[p_handle.id()].tree_id;
+	}
+	return 0;
+}
+
+public:
+void _handle_sort(BVHHandle &p_ha, BVHHandle &p_hb) const {
+	if (p_ha.id() > p_hb.id()) {
+		BVHHandle temp = p_hb;
+		p_hb = p_ha;
+		p_ha = temp;
+	}
+}
+
+private:
+void create_root_node(int p_tree) {
+	// if there is no root node, create one
+	if (_root_node_id[p_tree] == BVHCommon::INVALID) {
+		uint32_t root_node_id;
+		TNode *node = _nodes.request(root_node_id);
+		node->clear();
+		_root_node_id[p_tree] = root_node_id;
+
+		// make the root node a leaf
+		uint32_t leaf_id;
+		TLeaf *leaf = _leaves.request(leaf_id);
+		leaf->clear();
+		node->neg_leaf_id = -(int)leaf_id;
+	}
+}
+
+bool node_is_leaf_full(TNode &tnode) const {
+	const TLeaf &leaf = _node_get_leaf(tnode);
+	return leaf.is_full();
+}
+
+public:
+TLeaf &_node_get_leaf(TNode &tnode) {
+	BVH_ASSERT(tnode.is_leaf());
+	return _leaves[tnode.get_leaf_id()];
+}
+
+const TLeaf &_node_get_leaf(const TNode &tnode) const {
+	BVH_ASSERT(tnode.is_leaf());
+	return _leaves[tnode.get_leaf_id()];
+}
+
+private:

core/math/bvh_pair.inc

@@ -0,0 +1,72 @@
+public:
+// note .. maybe this can be attached to another node structure?
+// depends which works best for cache.
+struct ItemPairs {
+	struct Link {
+		void set(BVHHandle h, void *ud) {
+			handle = h;
+			userdata = ud;
+		}
+		BVHHandle handle;
+		void *userdata;
+	};
+
+	void clear() {
+		num_pairs = 0;
+		extended_pairs.reset();
+		expanded_aabb = BOUNDS();
+	}
+
+	BOUNDS expanded_aabb;
+
+	// maybe we can just use the number in the vector TODO
+	int32_t num_pairs;
+	LocalVector<Link> extended_pairs;
+
+	void add_pair_to(BVHHandle h, void *p_userdata) {
+		Link temp;
+		temp.set(h, p_userdata);
+
+		extended_pairs.push_back(temp);
+		num_pairs++;
+	}
+
+	uint32_t find_pair_to(BVHHandle h) const {
+		for (int n = 0; n < num_pairs; n++) {
+			if (extended_pairs[n].handle == h) {
+				return n;
+			}
+		}
+		return uint32_t(-1);
+	}
+
+	bool contains_pair_to(BVHHandle h) const {
+		return find_pair_to(h) != BVHCommon::INVALID;
+	}
+
+	// return success
+	void *remove_pair_to(BVHHandle h) {
+		void *userdata = nullptr;
+
+		for (int n = 0; n < num_pairs; n++) {
+			if (extended_pairs[n].handle == h) {
+				userdata = extended_pairs[n].userdata;
+				extended_pairs.remove_at_unordered(n);
+				num_pairs--;
+				break;
+			}
+		}
+
+		return userdata;
+	}
+
+	// experiment : scale the pairing expansion by the number of pairs.
+	// when the number of pairs is high, the density is high and a lower collision margin is better.
+	// when there are few local pairs, a larger margin is more optimal.
+	real_t scale_expansion_margin(real_t p_margin) const {
+		real_t x = real_t(num_pairs) * (1.0 / 9.0);
+		x = MIN(x, 1.0);
+		x = 1.0 - x;
+		return p_margin * x;
+	}
+};

core/math/bvh_public.inc

@@ -0,0 +1,499 @@
+public:
+BVHHandle item_add(T *p_userdata, bool p_active, const BOUNDS &p_aabb, int32_t p_subindex, uint32_t p_tree_id, uint32_t p_tree_collision_mask, bool p_invisible = false) {
+#ifdef BVH_VERBOSE_TREE
+	VERBOSE_PRINT("\nitem_add BEFORE");
+	_debug_recursive_print_tree(p_tree_id);
+	VERBOSE_PRINT("\n");
+#endif
+
+	BVHABB_CLASS abb;
+	abb.from(p_aabb);
+
+	// NOTE that we do not expand the AABB for the first create even if
+	// leaf expansion is switched on. This is for two reasons:
+	// (1) We don't know if this object will move in future, in which case a non-expanded
+	// bound would be better...
+	// (2) We don't yet know how many objects will be paired, which is used to modify
+	// the expansion margin.
+
+	// handle to be filled with the new item ref
+	BVHHandle handle;
+
+	// ref id easier to pass around than handle
+	uint32_t ref_id;
+
+	// this should never fail
+	ItemRef *ref = _refs.request(ref_id);
+
+	// the extra data should be parallel list to the references
+	uint32_t extra_id;
+	ItemExtra *extra = _extra.request(extra_id);
+	BVH_ASSERT(extra_id == ref_id);
+
+	// pairs info
+	if (USE_PAIRS) {
+		uint32_t pairs_id;
+		ItemPairs *pairs = _pairs.request(pairs_id);
+		pairs->clear();
+		BVH_ASSERT(pairs_id == ref_id);
+	}
+
+	extra->subindex = p_subindex;
+	extra->userdata = p_userdata;
+	extra->last_updated_tick = 0;
+
+	// add an active reference to the list for slow incremental optimize
+	// this list must be kept in sync with the references as they are added or removed.
+	extra->active_ref_id = _active_refs.size();
+	_active_refs.push_back(ref_id);
+
+	extra->tree_id = p_tree_id;
+	extra->tree_collision_mask = p_tree_collision_mask;
+
+	// assign to handle to return
+	handle.set_id(ref_id);
+
+	create_root_node(p_tree_id);
+
+	// we must choose where to add to tree
+	if (p_active) {
+		ref->tnode_id = _logic_choose_item_add_node(_root_node_id[p_tree_id], abb);
+
+		bool refit = _node_add_item(ref->tnode_id, ref_id, abb);
+
+		if (refit) {
+			// only need to refit from the parent
+			const TNode &add_node = _nodes[ref->tnode_id];
+			if (add_node.parent_id != BVHCommon::INVALID) {
+				refit_upward_and_balance(add_node.parent_id, p_tree_id);
+			}
+		}
+	} else {
+		ref->set_inactive();
+	}
+
+#ifdef BVH_VERBOSE
+	// memory use
+	int mem = _refs.estimate_memory_use();
+	mem += _nodes.estimate_memory_use();
+
+	String sz = _debug_aabb_to_string(abb);
+	VERBOSE_PRINT("\titem_add [" + itos(ref_id) + "] " + itos(_refs.used_size()) + " refs,\t" + itos(_nodes.used_size()) + " nodes " + sz);
+	VERBOSE_PRINT("mem use : " + itos(mem) + ", num nodes reserved : " + itos(_nodes.reserved_size()));
+
+#endif
+
+	return handle;
+}
+
+void _debug_print_refs() {
+#ifdef BVH_VERBOSE_TREE
+	print_line("refs.....");
+	for (int n = 0; n < _refs.size(); n++) {
+		const ItemRef &ref = _refs[n];
+		print_line("tnode_id " + itos(ref.tnode_id) + ", item_id " + itos(ref.item_id));
+	}
+
+#endif
+}
+
+// returns false if noop
+bool item_move(BVHHandle p_handle, const BOUNDS &p_aabb) {
+	uint32_t ref_id = p_handle.id();
+
+	// get the reference
+	ItemRef &ref = _refs[ref_id];
+	if (!ref.is_active()) {
+		return false;
+	}
+
+	BVHABB_CLASS abb;
+	abb.from(p_aabb);
+
+#ifdef BVH_EXPAND_LEAF_AABBS
+	if (USE_PAIRS) {
+		// scale the pairing expansion by the number of pairs.
+		abb.expand(_pairs[ref_id].scale_expansion_margin(_pairing_expansion));
+	} else {
+		abb.expand(_pairing_expansion);
+	}
+#endif
+
+	BVH_ASSERT(ref.tnode_id != BVHCommon::INVALID);
+	TNode &tnode = _nodes[ref.tnode_id];
+
+	// does it fit within the current leaf aabb?
+	if (tnode.aabb.is_other_within(abb)) {
+		// do nothing .. fast path .. not moved enough to need refit
+
+		// however we WILL update the exact aabb in the leaf, as this will be needed
+		// for accurate collision detection
+		TLeaf &leaf = _node_get_leaf(tnode);
+
+		BVHABB_CLASS &leaf_abb = leaf.get_aabb(ref.item_id);
+
+		// no change?
+#ifdef BVH_EXPAND_LEAF_AABBS
+		BOUNDS leaf_aabb;
+		leaf_abb.to(leaf_aabb);
+
+		// This test should pass in a lot of cases, and by returning false we can avoid
+		// collision pairing checks later, which greatly reduces processing.
+		if (expanded_aabb_encloses_not_shrink(leaf_aabb, p_aabb)) {
+			return false;
+		}
+#else
+		if (leaf_abb == abb) {
+			return false;
+		}
+#endif
+
+#ifdef BVH_VERBOSE_MOVES
+		print_line("item_move " + itos(p_handle.id()) + "(within tnode aabb) : " + _debug_aabb_to_string(abb));
+#endif
+
+		leaf_abb = abb;
+		_integrity_check_all();
+
+		return true;
+	}
+
+#ifdef BVH_VERBOSE_MOVES
+	print_line("item_move " + itos(p_handle.id()) + "(outside tnode aabb) : " + _debug_aabb_to_string(abb));
+#endif
+
+	uint32_t tree_id = _handle_get_tree_id(p_handle);
+
+	// remove and reinsert
+	node_remove_item(ref_id, tree_id);
+
+	// we must choose where to add to tree
+	ref.tnode_id = _logic_choose_item_add_node(_root_node_id[tree_id], abb);
+
+	// add to the tree
+	bool needs_refit = _node_add_item(ref.tnode_id, ref_id, abb);
+
+	// only need to refit from the PARENT
+	if (needs_refit) {
+		// only need to refit from the parent
+		const TNode &add_node = _nodes[ref.tnode_id];
+		if (add_node.parent_id != BVHCommon::INVALID) {
+			// not sure we need to rebalance all the time, this can be done less often
+			refit_upward(add_node.parent_id);
+		}
+		//refit_upward_and_balance(add_node.parent_id);
+	}
+
+	return true;
+}
+
+void item_remove(BVHHandle p_handle) {
+	uint32_t ref_id = p_handle.id();
+
+	uint32_t tree_id = _handle_get_tree_id(p_handle);
+
+	VERBOSE_PRINT("item_remove [" + itos(ref_id) + "] ");
+
+	////////////////////////////////////////
+	// remove the active reference from the list for slow incremental optimize
+	// this list must be kept in sync with the references as they are added or removed.
+	uint32_t active_ref_id = _extra[ref_id].active_ref_id;
+	uint32_t ref_id_moved_back = _active_refs[_active_refs.size() - 1];
+
+	// swap back and decrement for fast unordered remove
+	_active_refs[active_ref_id] = ref_id_moved_back;
+	_active_refs.resize_uninitialized(_active_refs.size() - 1);
+
+	// keep the moved active reference up to date
+	_extra[ref_id_moved_back].active_ref_id = active_ref_id;
+	////////////////////////////////////////
+
+	// remove the item from the node (only if active)
+	if (_refs[ref_id].is_active()) {
+		node_remove_item(ref_id, tree_id);
+	}
+
+	// remove the item reference
+	_refs.free(ref_id);
+	_extra.free(ref_id);
+	if (USE_PAIRS) {
+		_pairs.free(ref_id);
+	}
+
+	// don't think refit_all is necessary?
+	//refit_all(_tree_id);
+
+#ifdef BVH_VERBOSE_TREE
+	_debug_recursive_print_tree(tree_id);
+#endif
+}
+
+// returns success
+bool item_activate(BVHHandle p_handle, const BOUNDS &p_aabb) {
+	uint32_t ref_id = p_handle.id();
+	ItemRef &ref = _refs[ref_id];
+	if (ref.is_active()) {
+		// noop
+		return false;
+	}
+
+	// add to tree
+	BVHABB_CLASS abb;
+	abb.from(p_aabb);
+
+	uint32_t tree_id = _handle_get_tree_id(p_handle);
+
+	// we must choose where to add to tree
+	ref.tnode_id = _logic_choose_item_add_node(_root_node_id[tree_id], abb);
+	_node_add_item(ref.tnode_id, ref_id, abb);
+
+	refit_upward_and_balance(ref.tnode_id, tree_id);
+
+	return true;
+}
+
+// returns success
+bool item_deactivate(BVHHandle p_handle) {
+	uint32_t ref_id = p_handle.id();
+	ItemRef &ref = _refs[ref_id];
+	if (!ref.is_active()) {
+		// noop
+		return false;
+	}
+
+	uint32_t tree_id = _handle_get_tree_id(p_handle);
+
+	// remove from tree
+	BVHABB_CLASS abb;
+	node_remove_item(ref_id, tree_id, &abb);
+
+	// mark as inactive
+	ref.set_inactive();
+	return true;
+}
+
+bool item_get_active(BVHHandle p_handle) const {
+	uint32_t ref_id = p_handle.id();
+	const ItemRef &ref = _refs[ref_id];
+	return ref.is_active();
+}
+
+// during collision testing, we want to set the mask and whether pairable for the item testing from
+void item_fill_cullparams(BVHHandle p_handle, CullParams &r_params) const {
+	uint32_t ref_id = p_handle.id();
+	const ItemExtra &extra = _extra[ref_id];
+
+	// which trees does this item want to collide detect against?
+	r_params.tree_collision_mask = extra.tree_collision_mask;
+
+	// The testing user defined object is passed to the user defined cull check function
+	// for masks etc. This is usually a dummy object of type T with masks set.
+	// However, if not using the cull_check callback (i.e. returning true), you can pass
+	// a nullptr instead of dummy object, as it will not be used.
+	r_params.tester = extra.userdata;
+}
+
+bool item_is_pairable(const BVHHandle &p_handle) {
+	uint32_t ref_id = p_handle.id();
+	const ItemExtra &extra = _extra[ref_id];
+	return extra.pairable != 0;
+}
+
+void item_get_ABB(const BVHHandle &p_handle, BVHABB_CLASS &r_abb) {
+	// change tree?
+	uint32_t ref_id = p_handle.id();
+	const ItemRef &ref = _refs[ref_id];
+
+	TNode &tnode = _nodes[ref.tnode_id];
+	TLeaf &leaf = _node_get_leaf(tnode);
+
+	r_abb = leaf.get_aabb(ref.item_id);
+}
+
+bool item_set_tree(const BVHHandle &p_handle, uint32_t p_tree_id, uint32_t p_tree_collision_mask) {
+	// change tree?
+	uint32_t ref_id = p_handle.id();
+
+	ItemExtra &ex = _extra[ref_id];
+	ItemRef &ref = _refs[ref_id];
+
+	bool active = ref.is_active();
+	bool tree_changed = ex.tree_id != p_tree_id;
+	bool mask_changed = ex.tree_collision_mask != p_tree_collision_mask;
+	bool state_changed = tree_changed | mask_changed;
+
+	// Keep an eye on this for bugs of not noticing changes to objects,
+	// especially when changing client user masks that will not be detected as a change
+	// in the BVH. You may need to force a collision check in this case with recheck_pairs().
+
+	if (active && (tree_changed | mask_changed)) {
+		// record abb
+		TNode &tnode = _nodes[ref.tnode_id];
+		TLeaf &leaf = _node_get_leaf(tnode);
+		BVHABB_CLASS abb = leaf.get_aabb(ref.item_id);
+
+		// make sure current tree is correct prior to changing
+		uint32_t tree_id = _handle_get_tree_id(p_handle);
+
+		// remove from old tree
+		node_remove_item(ref_id, tree_id);
+
+		// we must set the pairable AFTER getting the current tree
+		// because the pairable status determines which tree
+		ex.tree_id = p_tree_id;
+		ex.tree_collision_mask = p_tree_collision_mask;
+
+		// add to new tree
+		tree_id = _handle_get_tree_id(p_handle);
+		create_root_node(tree_id);
+
+		// we must choose where to add to tree
+		ref.tnode_id = _logic_choose_item_add_node(_root_node_id[tree_id], abb);
+		bool needs_refit = _node_add_item(ref.tnode_id, ref_id, abb);
+
+		// only need to refit from the PARENT
+		if (needs_refit) {
+			// only need to refit from the parent
+			const TNode &add_node = _nodes[ref.tnode_id];
+			if (add_node.parent_id != BVHCommon::INVALID) {
+				refit_upward_and_balance(add_node.parent_id, tree_id);
+			}
+		}
+	} else {
+		// always keep this up to date
+		ex.tree_id = p_tree_id;
+		ex.tree_collision_mask = p_tree_collision_mask;
+	}
+
+	return state_changed;
+}
+
+void incremental_optimize() {
+	// first update all aabbs as one off step..
+	// this is cheaper than doing it on each move as each leaf may get touched multiple times
+	// in a frame.
+	for (int n = 0; n < NUM_TREES; n++) {
+		if (_root_node_id[n] != BVHCommon::INVALID) {
+			refit_branch(_root_node_id[n]);
+		}
+	}
+
+	// now do small section reinserting to get things moving
+	// gradually, and keep items in the right leaf
+	if (_current_active_ref >= _active_refs.size()) {
+		_current_active_ref = 0;
+	}
+
+	// special case
+	if (!_active_refs.size()) {
+		return;
+	}
+
+	uint32_t ref_id = _active_refs[_current_active_ref++];
+
+	_logic_item_remove_and_reinsert(ref_id);
+
+#ifdef BVH_VERBOSE
+	/*
+	// memory use
+	int mem_refs = _refs.estimate_memory_use();
+	int mem_nodes = _nodes.estimate_memory_use();
+	int mem_leaves = _leaves.estimate_memory_use();
+
+	String sz;
+	sz += "mem_refs : " + itos(mem_refs) + " ";
+	sz += "mem_nodes : " + itos(mem_nodes) + " ";
+	sz += "mem_leaves : " + itos(mem_leaves) + " ";
+	sz += ", num nodes : " + itos(_nodes.size());
+	print_line(sz);
+	*/
+#endif
+}
+
+void update() {
+	incremental_optimize();
+
+	// keep the expansion values up to date with the world bound
+//#define BVH_ALLOW_AUTO_EXPANSION
+#ifdef BVH_ALLOW_AUTO_EXPANSION
+	if (_auto_node_expansion || _auto_pairing_expansion) {
+		BVHABB_CLASS world_bound;
+		world_bound.set_to_max_opposite_extents();
+
+		bool bound_valid = false;
+
+		for (int n = 0; n < NUM_TREES; n++) {
+			uint32_t node_id = _root_node_id[n];
+			if (node_id != BVHCommon::INVALID) {
+				world_bound.merge(_nodes[node_id].aabb);
+				bound_valid = true;
+			}
+		}
+
+		// if there are no nodes, do nothing, but if there are...
+		if (bound_valid) {
+			BOUNDS bb;
+			world_bound.to(bb);
+			real_t size = bb.get_longest_axis_size();
+
+			// automatic AI decision for best parameters.
+			// These can be overridden in project settings.
+
+			// these magic numbers are determined by experiment
+			if (_auto_node_expansion) {
+				_node_expansion = size * 0.025;
+			}
+			if (_auto_pairing_expansion) {
+				_pairing_expansion = size * 0.009;
+			}
+		}
+	}
+#endif
+}
+
+void params_set_pairing_expansion(real_t p_value) {
+	if (p_value < 0.0) {
+#ifdef BVH_ALLOW_AUTO_EXPANSION
+		_auto_pairing_expansion = true;
+#endif
+		return;
+	}
+#ifdef BVH_ALLOW_AUTO_EXPANSION
+	_auto_pairing_expansion = false;
+#endif
+
+	_pairing_expansion = p_value;
+
+	// calculate shrinking threshold
+	const real_t fudge_factor = 1.1;
+	_aabb_shrinkage_threshold = _pairing_expansion * POINT::AXIS_COUNT * 2.0 * fudge_factor;
+}
+
+// This routine is not just an enclose check, it also checks for special case of shrinkage
+bool expanded_aabb_encloses_not_shrink(const BOUNDS &p_expanded_aabb, const BOUNDS &p_aabb) const {
+	if (!p_expanded_aabb.encloses(p_aabb)) {
+		return false;
+	}
+
+	// Check for special case of shrinkage. If the aabb has shrunk
+	// significantly we want to create a new expanded bound, because
+	// the previous expanded bound will have diverged significantly.
+	const POINT &exp_size = p_expanded_aabb.size;
+	const POINT &new_size = p_aabb.size;
+
+	real_t exp_l = 0.0;
+	real_t new_l = 0.0;
+
+	for (int i = 0; i < POINT::AXIS_COUNT; ++i) {
+		exp_l += exp_size[i];
+		new_l += new_size[i];
+	}
+
+	// is difference above some metric
+	real_t diff = exp_l - new_l;
+	if (diff < _aabb_shrinkage_threshold) {
+		return true;
+	}
+
+	return false;
+}

core/math/bvh_refit.inc

@@ -0,0 +1,141 @@
+void _debug_node_verify_bound(uint32_t p_node_id) {
+	TNode &node = _nodes[p_node_id];
+	BVHABB_CLASS abb_before = node.aabb;
+
+	node_update_aabb(node);
+
+	BVHABB_CLASS abb_after = node.aabb;
+	CRASH_COND(abb_before != abb_after);
+}
+
+void node_update_aabb(TNode &tnode) {
+	tnode.aabb.set_to_max_opposite_extents();
+	tnode.height = 0;
+
+	if (!tnode.is_leaf()) {
+		for (int n = 0; n < tnode.num_children; n++) {
+			uint32_t child_node_id = tnode.children[n];
+
+			// merge with child aabb
+			const TNode &tchild = _nodes[child_node_id];
+			tnode.aabb.merge(tchild.aabb);
+
+			// do heights at the same time
+			if (tchild.height > tnode.height) {
+				tnode.height = tchild.height;
+			}
+		}
+
+		// the height of a non leaf is always 1 bigger than the biggest child
+		tnode.height++;
+
+#ifdef BVH_CHECKS
+		if (!tnode.num_children) {
+			// the 'blank' aabb will screw up parent aabbs
+			WARN_PRINT("BVH_Tree::TNode no children, AABB is undefined");
+		}
+#endif
+	} else {
+		// leaf
+		const TLeaf &leaf = _node_get_leaf(tnode);
+
+		for (int n = 0; n < leaf.num_items; n++) {
+			tnode.aabb.merge(leaf.get_aabb(n));
+		}
+
+		// now the leaf items are unexpanded, we expand only in the node AABB
+		tnode.aabb.expand(_node_expansion);
+#ifdef BVH_CHECKS
+		if (!leaf.num_items) {
+			// the 'blank' aabb will screw up parent aabbs
+			WARN_PRINT("BVH_Tree::TLeaf no items, AABB is undefined");
+		}
+#endif
+	}
+}
+
+void refit_all(int p_tree_id) {
+	refit_downward(_root_node_id[p_tree_id]);
+}
+
+void refit_upward(uint32_t p_node_id) {
+	while (p_node_id != BVHCommon::INVALID) {
+		TNode &tnode = _nodes[p_node_id];
+		node_update_aabb(tnode);
+		p_node_id = tnode.parent_id;
+	}
+}
+
+void refit_upward_and_balance(uint32_t p_node_id, uint32_t p_tree_id) {
+	while (p_node_id != BVHCommon::INVALID) {
+		uint32_t before = p_node_id;
+		p_node_id = _logic_balance(p_node_id, p_tree_id);
+
+		if (before != p_node_id) {
+			VERBOSE_PRINT("REBALANCED!");
+		}
+
+		TNode &tnode = _nodes[p_node_id];
+
+		// update overall aabb from the children
+		node_update_aabb(tnode);
+
+		p_node_id = tnode.parent_id;
+	}
+}
+
+void refit_downward(uint32_t p_node_id) {
+	TNode &tnode = _nodes[p_node_id];
+
+	// do children first
+	if (!tnode.is_leaf()) {
+		for (int n = 0; n < tnode.num_children; n++) {
+			refit_downward(tnode.children[n]);
+		}
+	}
+
+	node_update_aabb(tnode);
+}
+
+// go down to the leaves, then refit upward
+void refit_branch(uint32_t p_node_id) {
+	// our function parameters to keep on a stack
+	struct RefitParams {
+		uint32_t node_id;
+	};
+
+	// most of the iterative functionality is contained in this helper class
+	BVH_IterativeInfo<RefitParams> ii;
+
+	// alloca must allocate the stack from this function, it cannot be allocated in the
+	// helper class
+	ii.stack = (RefitParams *)alloca(ii.get_alloca_stacksize());
+
+	// seed the stack
+	ii.get_first()->node_id = p_node_id;
+
+	RefitParams rp;
+
+	// while there are still more nodes on the stack
+	while (ii.pop(rp)) {
+		TNode &tnode = _nodes[rp.node_id];
+
+		// do children first
+		if (!tnode.is_leaf()) {
+			for (int n = 0; n < tnode.num_children; n++) {
+				uint32_t child_id = tnode.children[n];
+
+				// add to the stack
+				RefitParams *child = ii.request();
+				child->node_id = child_id;
+			}
+		} else {
+			// leaf .. only refit upward if dirty
+			TLeaf &leaf = _node_get_leaf(tnode);
+			if (leaf.is_dirty()) {
+				leaf.set_dirty(false);
+				refit_upward(rp.node_id);
+			}
+		}
+	} // while more nodes to pop
+}

core/math/bvh_split.inc

@@ -0,0 +1,298 @@
+void _split_inform_references(uint32_t p_node_id) {
+	TNode &node = _nodes[p_node_id];
+	TLeaf &leaf = _node_get_leaf(node);
+
+	for (int n = 0; n < leaf.num_items; n++) {
+		uint32_t ref_id = leaf.get_item_ref_id(n);
+
+		ItemRef &ref = _refs[ref_id];
+		ref.tnode_id = p_node_id;
+		ref.item_id = n;
+	}
+}
+
+void _split_leaf_sort_groups_simple(int &num_a, int &num_b, uint16_t *group_a, uint16_t *group_b, const BVHABB_CLASS *temp_bounds, const BVHABB_CLASS full_bound) {
+	// special case for low leaf sizes .. should static compile out
+	if constexpr (MAX_ITEMS < 4) {
+		uint32_t ind = group_a[0];
+
+		// add to b
+		group_b[num_b++] = ind;
+
+		// remove from a
+		num_a--;
+		group_a[0] = group_a[num_a];
+		return;
+	}
+
+	POINT center = full_bound.calculate_center();
+	POINT size = full_bound.calculate_size();
+
+	int order[POINT::AXIS_COUNT];
+
+	order[0] = size.max_axis_index(); // The longest axis.
+	order[POINT::AXIS_COUNT - 1] = size.min_axis_index(); // The shortest axis.
+
+	static_assert(POINT::AXIS_COUNT <= 3, "BVH POINT::AXIS_COUNT has unexpected size");
+	if constexpr (POINT::AXIS_COUNT == 3) {
+		order[1] = 3 - (order[0] + order[2]);
+	}
+
+	// Simplest case, split on the longest axis.
+	int split_axis = order[0];
+	for (int a = 0; a < num_a; a++) {
+		uint32_t ind = group_a[a];
+
+		if (temp_bounds[ind].min.coord[split_axis] > center.coord[split_axis]) {
+			// add to b
+			group_b[num_b++] = ind;
+
+			// remove from a
+			num_a--;
+			group_a[a] = group_a[num_a];
+
+			// do this one again, as it has been replaced
+			a--;
+		}
+	}
+
+	// detect when split on longest axis failed
+	int min_threshold = MAX_ITEMS / 4;
+	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
+		for (int b = 0; b < num_b; b++) {
+			group_a[num_a++] = group_b[b];
+		}
+		num_b = 0;
+
+		// Now calculate the best split.
+		for (int axis = 1; axis < POINT::AXIS_COUNT; axis++) {
+			split_axis = order[axis];
+			int count = 0;
+
+			for (int a = 0; a < num_a; a++) {
+				uint32_t ind = group_a[a];
+
+				if (temp_bounds[ind].min.coord[split_axis] > center.coord[split_axis]) {
+					count++;
+				}
+			}
+
+			min_group_size[axis] = MIN(count, num_a - count);
+		} // for axis
+
+		// best axis
+		int best_axis = 0;
+		int best_min = min_group_size[0];
+		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;
+			}
+		}
+
+		// now finally do the split
+		if (best_min > 0) {
+			split_axis = order[best_axis];
+
+			for (int a = 0; a < num_a; a++) {
+				uint32_t ind = group_a[a];
+
+				if (temp_bounds[ind].min.coord[split_axis] > center.coord[split_axis]) {
+					// add to b
+					group_b[num_b++] = ind;
+
+					// remove from a
+					num_a--;
+					group_a[a] = group_a[num_a];
+
+					// do this one again, as it has been replaced
+					a--;
+				}
+			}
+		} // if there was a split!
+	} // if the longest axis wasn't a good split
+
+	// special case, none crossed threshold
+	if (!num_b) {
+		uint32_t ind = group_a[0];
+
+		// add to b
+		group_b[num_b++] = ind;
+
+		// remove from a
+		num_a--;
+		group_a[0] = group_a[num_a];
+	}
+	// opposite problem! :)
+	if (!num_a) {
+		uint32_t ind = group_b[0];
+
+		// add to a
+		group_a[num_a++] = ind;
+
+		// remove from b
+		num_b--;
+		group_b[0] = group_b[num_b];
+	}
+}
+
+void _split_leaf_sort_groups(int &num_a, int &num_b, uint16_t *group_a, uint16_t *group_b, const BVHABB_CLASS *temp_bounds) {
+	BVHABB_CLASS groupb_aabb;
+	groupb_aabb.set_to_max_opposite_extents();
+	for (int n = 0; n < num_b; n++) {
+		int which = group_b[n];
+		groupb_aabb.merge(temp_bounds[which]);
+	}
+	BVHABB_CLASS groupb_aabb_new;
+
+	BVHABB_CLASS rest_aabb;
+
+	real_t best_size = FLT_MAX;
+	int best_candidate = -1;
+
+	// find most likely from a to move into b
+	for (int check = 0; check < num_a; check++) {
+		rest_aabb.set_to_max_opposite_extents();
+		groupb_aabb_new = groupb_aabb;
+
+		// find aabb of all the rest
+		for (int rest = 0; rest < num_a; rest++) {
+			if (rest == check) {
+				continue;
+			}
+
+			int which = group_a[rest];
+			rest_aabb.merge(temp_bounds[which]);
+		}
+
+		groupb_aabb_new.merge(temp_bounds[group_a[check]]);
+
+		// now compare the sizes
+		real_t size = groupb_aabb_new.get_area() + rest_aabb.get_area();
+		if (size < best_size) {
+			best_size = size;
+			best_candidate = check;
+		}
+	}
+
+	// we should now have the best, move it from group a to group b
+	group_b[num_b++] = group_a[best_candidate];
+
+	// remove best candidate from group a
+	num_a--;
+	group_a[best_candidate] = group_a[num_a];
+}
+
+uint32_t split_leaf(uint32_t p_node_id, const BVHABB_CLASS &p_added_item_aabb) {
+	return split_leaf_complex(p_node_id, p_added_item_aabb);
+}
+
+// aabb is the new inserted node
+uint32_t split_leaf_complex(uint32_t p_node_id, const BVHABB_CLASS &p_added_item_aabb) {
+	VERBOSE_PRINT("split_leaf");
+
+	// note the tnode before and AFTER splitting may be a different address
+	// in memory because the vector could get relocated. So we need to reget
+	// the tnode after the split
+	BVH_ASSERT(_nodes[p_node_id].is_leaf());
+
+	// first create child leaf nodes
+	uint32_t *child_ids = (uint32_t *)alloca(sizeof(uint32_t) * MAX_CHILDREN);
+
+	for (int n = 0; n < MAX_CHILDREN; n++) {
+		// create node children
+		TNode *child_node = _nodes.request(child_ids[n]);
+
+		child_node->clear();
+
+		// back link to parent
+		child_node->parent_id = p_node_id;
+
+		// make each child a leaf node
+		node_make_leaf(child_ids[n]);
+	}
+
+	// don't get any leaves or nodes till AFTER the split
+	TNode &tnode = _nodes[p_node_id];
+	uint32_t orig_leaf_id = tnode.get_leaf_id();
+	const TLeaf &orig_leaf = _node_get_leaf(tnode);
+
+	// store the final child ids
+	for (int n = 0; n < MAX_CHILDREN; n++) {
+		tnode.children[n] = child_ids[n];
+	}
+
+	// mark as no longer a leaf node
+	tnode.num_children = MAX_CHILDREN;
+
+	// 2 groups, A and B, and assign children to each to split equally
+	int max_children = orig_leaf.num_items + 1; // plus 1 for the wildcard .. the item being added
+	//CRASH_COND(max_children > MAX_CHILDREN);
+
+	uint16_t *group_a = (uint16_t *)alloca(sizeof(uint16_t) * max_children);
+	uint16_t *group_b = (uint16_t *)alloca(sizeof(uint16_t) * max_children);
+
+	// we are copying the ABBs. This is ugly, but we need one extra for the inserted item...
+	BVHABB_CLASS *temp_bounds = (BVHABB_CLASS *)alloca(sizeof(BVHABB_CLASS) * max_children);
+
+	int num_a = max_children;
+	int num_b = 0;
+
+	// setup - start with all in group a
+	for (int n = 0; n < orig_leaf.num_items; n++) {
+		group_a[n] = n;
+		temp_bounds[n] = orig_leaf.get_aabb(n);
+	}
+	// wildcard
+	int wildcard = orig_leaf.num_items;
+
+	group_a[wildcard] = wildcard;
+	temp_bounds[wildcard] = p_added_item_aabb;
+
+	// we can choose here either an equal split, or just 1 in the new leaf
+	_split_leaf_sort_groups_simple(num_a, num_b, group_a, group_b, temp_bounds, tnode.aabb);
+
+	uint32_t wildcard_node = BVHCommon::INVALID;
+
+	// now there should be equal numbers in both groups
+	for (int n = 0; n < num_a; n++) {
+		int which = group_a[n];
+
+		if (which != wildcard) {
+			const BVHABB_CLASS &source_item_aabb = orig_leaf.get_aabb(which);
+			uint32_t source_item_ref_id = orig_leaf.get_item_ref_id(which);
+			//const Item &source_item = orig_leaf.get_item(which);
+			_node_add_item(tnode.children[0], source_item_ref_id, source_item_aabb);
+		} else {
+			wildcard_node = tnode.children[0];
+		}
+	}
+	for (int n = 0; n < num_b; n++) {
+		int which = group_b[n];
+
+		if (which != wildcard) {
+			const BVHABB_CLASS &source_item_aabb = orig_leaf.get_aabb(which);
+			uint32_t source_item_ref_id = orig_leaf.get_item_ref_id(which);
+			//const Item &source_item = orig_leaf.get_item(which);
+			_node_add_item(tnode.children[1], source_item_ref_id, source_item_aabb);
+		} else {
+			wildcard_node = tnode.children[1];
+		}
+	}
+
+	// now remove all items from the parent and replace with the child nodes
+	_leaves.free(orig_leaf_id);
+
+	// we should keep the references up to date!
+	for (int n = 0; n < MAX_CHILDREN; n++) {
+		_split_inform_references(tnode.children[n]);
+	}
+
+	refit_upward(p_node_id);
+
+	BVH_ASSERT(wildcard_node != BVHCommon::INVALID);
+	return wildcard_node;
+}

core/math/bvh_structs.inc

@@ -0,0 +1,214 @@
+public:
+struct ItemRef {
+	uint32_t tnode_id; // -1 is invalid
+	uint32_t item_id; // in the leaf
+
+	bool is_active() const { return tnode_id != BVHCommon::INACTIVE; }
+	void set_inactive() {
+		tnode_id = BVHCommon::INACTIVE;
+		item_id = BVHCommon::INACTIVE;
+	}
+};
+
+// extra info kept in separate parallel list to the references,
+// as this is less used as keeps cache better
+struct ItemExtra {
+	// Before doing user defined pairing checks (especially in the find_leavers function),
+	// we may want to check that two items have compatible tree ids and tree masks,
+	// as if they are incompatible they should not pair / collide.
+	bool are_item_trees_compatible(const ItemExtra &p_other) const {
+		uint32_t other_type = 1 << p_other.tree_id;
+		if (tree_collision_mask & other_type) {
+			return true;
+		}
+		uint32_t our_type = 1 << tree_id;
+		if (p_other.tree_collision_mask & our_type) {
+			return true;
+		}
+		return false;
+	}
+
+	// There can be multiple user defined trees
+	uint32_t tree_id;
+
+	// Defines which trees this item should collision check against.
+	// 1 << tree_id, and normally items would collide against there own
+	// tree (but not always).
+	uint32_t tree_collision_mask;
+
+	uint32_t last_updated_tick;
+	int32_t subindex;
+
+	T *userdata;
+
+	// the active reference is a separate list of which references
+	// are active so that we can slowly iterate through it over many frames for
+	// slow optimize.
+	uint32_t active_ref_id;
+};
+
+// tree leaf
+struct TLeaf {
+	uint16_t num_items;
+
+private:
+	uint16_t dirty;
+	// separate data orientated lists for faster SIMD traversal
+	uint32_t item_ref_ids[MAX_ITEMS];
+	BVHABB_CLASS aabbs[MAX_ITEMS];
+
+public:
+	// accessors
+	BVHABB_CLASS &get_aabb(uint32_t p_id) {
+		BVH_ASSERT(p_id < MAX_ITEMS);
+		return aabbs[p_id];
+	}
+	const BVHABB_CLASS &get_aabb(uint32_t p_id) const {
+		BVH_ASSERT(p_id < MAX_ITEMS);
+		return aabbs[p_id];
+	}
+
+	uint32_t &get_item_ref_id(uint32_t p_id) {
+		BVH_ASSERT(p_id < MAX_ITEMS);
+		return item_ref_ids[p_id];
+	}
+	const uint32_t &get_item_ref_id(uint32_t p_id) const {
+		BVH_ASSERT(p_id < MAX_ITEMS);
+		return item_ref_ids[p_id];
+	}
+
+	bool is_dirty() const { return dirty; }
+	void set_dirty(bool p) { dirty = p; }
+
+	void clear() {
+		num_items = 0;
+		set_dirty(false);
+	}
+	bool is_full() const { return num_items >= MAX_ITEMS; }
+
+	void remove_item_unordered(uint32_t p_id) {
+		BVH_ASSERT(p_id < num_items);
+		num_items--;
+		aabbs[p_id] = aabbs[num_items];
+		item_ref_ids[p_id] = item_ref_ids[num_items];
+	}
+
+	uint32_t request_item() {
+		if (num_items < MAX_ITEMS) {
+			uint32_t id = num_items;
+			num_items++;
+			return id;
+		}
+#ifdef DEV_ENABLED
+		return -1;
+#else
+		ERR_FAIL_V_MSG(0, "BVH request_item error.");
+#endif
+	}
+};
+
+// tree node
+struct TNode {
+	BVHABB_CLASS aabb;
+	// either number of children if positive
+	// or leaf id if negative (leaf id 0 is disallowed)
+	union {
+		int32_t num_children;
+		int32_t neg_leaf_id;
+	};
+	uint32_t parent_id; // or -1
+	uint16_t children[MAX_CHILDREN];
+
+	// height in the tree, where leaves are 0, and all above are 1+
+	// (or the highest where there is a tie off)
+	int32_t height;
+
+	bool is_leaf() const { return num_children < 0; }
+	void set_leaf_id(int id) { neg_leaf_id = -id; }
+	int get_leaf_id() const { return -neg_leaf_id; }
+
+	void clear() {
+		num_children = 0;
+		parent_id = BVHCommon::INVALID;
+		height = 0; // or -1 for testing
+
+		// for safety set to improbable value
+		aabb.set_to_max_opposite_extents();
+
+		// other members are not blanked for speed .. they may be uninitialized
+	}
+
+	bool is_full_of_children() const { return num_children >= MAX_CHILDREN; }
+
+	void remove_child_internal(uint32_t child_num) {
+		children[child_num] = children[num_children - 1];
+		num_children--;
+	}
+
+	int find_child(uint32_t p_child_node_id) {
+		BVH_ASSERT(!is_leaf());
+
+		for (int n = 0; n < num_children; n++) {
+			if (children[n] == p_child_node_id) {
+				return n;
+			}
+		}
+
+		// not found
+		return -1;
+	}
+};
+
+// instead of using linked list we maintain
+// item references (for quick lookup)
+PooledList<ItemRef, uint32_t, true> _refs;
+PooledList<ItemExtra, uint32_t, true> _extra;
+PooledList<ItemPairs> _pairs;
+
+// these 2 are not in sync .. nodes != leaves!
+PooledList<TNode, uint32_t, true> _nodes;
+PooledList<TLeaf, uint32_t, true> _leaves;
+
+// we can maintain an un-ordered list of which references are active,
+// in order to do a slow incremental optimize of the tree over each frame.
+// This will work best if dynamic objects and static objects are in a different tree.
+LocalVector<uint32_t> _active_refs;
+uint32_t _current_active_ref = 0;
+
+// instead of translating directly to the userdata output,
+// we keep an intermediate list of hits as reference IDs, which can be used
+// for pairing collision detection
+LocalVector<uint32_t> _cull_hits;
+
+// We can now have a user definable number of trees.
+// This allows using e.g. a non-pairable and pairable tree,
+// which can be more efficient for example, if we only need check non pairable against the pairable tree.
+// It also may be more efficient in terms of separating static from dynamic objects, by reducing housekeeping.
+// However this is a trade off, as there is a cost of traversing two trees.
+uint32_t _root_node_id[NUM_TREES];
+
+// these values may need tweaking according to the project
+// the bound of the world, and the average velocities of the objects
+
+// node expansion is important in the rendering tree
+// larger values give less re-insertion as items move...
+// but on the other hand over estimates the bounding box of nodes.
+// we can either use auto mode, where the expansion is based on the root node size, or specify manually
+real_t _node_expansion = 0.5;
+bool _auto_node_expansion = true;
+
+// pairing expansion important for physics pairing
+// larger values gives more 'sticky' pairing, and is less likely to exhibit tunneling
+// we can either use auto mode, where the expansion is based on the root node size, or specify manually
+real_t _pairing_expansion = 0.1;
+
+#ifdef BVH_ALLOW_AUTO_EXPANSION
+bool _auto_pairing_expansion = true;
+#endif
+
+// when using an expanded bound, we must detect the condition where a new AABB
+// is significantly smaller than the expanded bound, as this is a special case where we
+// should override the optimization and create a new expanded bound.
+// This threshold is derived from the _pairing_expansion, and should be recalculated
+// if _pairing_expansion is changed.
+real_t _aabb_shrinkage_threshold = 0.0;

core/math/bvh_tree.h

@@ -0,0 +1,455 @@
+/**************************************************************************/
+/*  bvh_tree.h                                                            */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#pragma once
+
+// BVH Tree
+// This is an implementation of a dynamic BVH with templated leaf size.
+// This differs from most dynamic BVH in that it can handle more than 1 object
+// in leaf nodes. This can make it far more efficient in certain circumstances.
+// It also means that the splitting logic etc have to be completely different
+// to a simpler tree.
+// Note that MAX_CHILDREN should be fixed at 2 for now.
+
+#include "core/math/aabb.h"
+#include "core/math/bvh_abb.h"
+#include "core/math/vector3.h"
+#include "core/templates/local_vector.h"
+#include "core/templates/pooled_list.h"
+
+#include <climits>
+
+#define BVHABB_CLASS BVH_ABB<BOUNDS, POINT>
+
+// not sure if this is better yet so making optional
+#define BVH_EXPAND_LEAF_AABBS
+
+// never do these checks in release
+#ifdef DEV_ENABLED
+//#define BVH_VERBOSE
+//#define BVH_VERBOSE_TREE
+//#define BVH_VERBOSE_PAIRING
+//#define BVH_VERBOSE_MOVES
+
+//#define BVH_VERBOSE_FRAME
+//#define BVH_CHECKS
+//#define BVH_INTEGRITY_CHECKS
+#endif
+
+// debug only assert
+#ifdef BVH_CHECKS
+#define BVH_ASSERT(a) CRASH_COND((a) == false)
+#else
+#define BVH_ASSERT(a)
+#endif
+
+#ifdef BVH_VERBOSE
+#define VERBOSE_PRINT print_line
+#else
+#define VERBOSE_PRINT(a)
+#endif
+
+// really just a namespace
+struct BVHCommon {
+	// these could possibly also be the same constant,
+	// although this may be useful for debugging.
+	// or use zero for invalid and +1 based indices.
+	static const uint32_t INVALID = (0xffffffff);
+	static const uint32_t INACTIVE = (0xfffffffe);
+};
+
+// really a handle, can be anything
+// note that zero is a valid reference for the BVH .. this may involve using
+// a plus one based ID for clients that expect 0 to be invalid.
+struct BVHHandle {
+	// conversion operator
+	operator uint32_t() const { return _data; }
+	void set(uint32_t p_value) { _data = p_value; }
+
+	uint32_t _data;
+
+	void set_invalid() { _data = BVHCommon::INVALID; }
+	bool is_invalid() const { return _data == BVHCommon::INVALID; }
+	uint32_t id() const { return _data; }
+	void set_id(uint32_t p_id) { _data = p_id; }
+
+	bool operator==(const BVHHandle &p_h) const { return _data == p_h._data; }
+	bool operator!=(const BVHHandle &p_h) const { return (*this == p_h) == false; }
+};
+
+// helper class to make iterative versions of recursive functions
+template <typename T>
+class BVH_IterativeInfo {
+public:
+	constexpr static const size_t ALLOCA_STACK_SIZE = 128;
+
+	int32_t depth = 1;
+	int32_t threshold = ALLOCA_STACK_SIZE - 2;
+	T *stack;
+	//only used in rare occasions when you run out of alloca memory
+	// because tree is too unbalanced.
+	LocalVector<T> aux_stack;
+	int32_t get_alloca_stacksize() const { return ALLOCA_STACK_SIZE * sizeof(T); }
+
+	T *get_first() const {
+		return &stack[0];
+	}
+
+	// pop the last member of the stack, or return false
+	bool pop(T &r_value) {
+		if (!depth) {
+			return false;
+		}
+
+		depth--;
+		r_value = stack[depth];
+		return true;
+	}
+
+	// request new addition to stack
+	T *request() {
+		if (depth > threshold) {
+			if (aux_stack.is_empty()) {
+				aux_stack.resize(ALLOCA_STACK_SIZE * 2);
+				memcpy(aux_stack.ptr(), stack, get_alloca_stacksize());
+			} else {
+				aux_stack.resize(aux_stack.size() * 2);
+			}
+			stack = aux_stack.ptr();
+			threshold = aux_stack.size() - 2;
+		}
+		return &stack[depth++];
+	}
+};
+
+template <typename T>
+class BVH_DummyPairTestFunction {
+public:
+	static bool user_collision_check(T *p_a, T *p_b) {
+		// return false if no collision, decided by masks etc
+		return true;
+	}
+};
+
+template <typename T>
+class BVH_DummyCullTestFunction {
+public:
+	static bool user_cull_check(T *p_a, T *p_b) {
+		// return false if no collision
+		return true;
+	}
+};
+
+template <typename T, int NUM_TREES, int MAX_CHILDREN, int MAX_ITEMS, typename USER_PAIR_TEST_FUNCTION = BVH_DummyPairTestFunction<T>, typename USER_CULL_TEST_FUNCTION = BVH_DummyCullTestFunction<T>, bool USE_PAIRS = false, typename BOUNDS = AABB, typename POINT = Vector3>
+class BVH_Tree {
+	friend class BVH;
+
+#include "bvh_pair.inc"
+#include "bvh_structs.inc"
+
+public:
+	BVH_Tree() {
+		for (int n = 0; n < NUM_TREES; n++) {
+			_root_node_id[n] = BVHCommon::INVALID;
+		}
+
+		// disallow zero leaf ids
+		// (as these ids are stored as negative numbers in the node)
+		uint32_t dummy_leaf_id;
+		_leaves.request(dummy_leaf_id);
+
+		// In many cases you may want to change this default in the client code,
+		// or expose this value to the user.
+		// This default may make sense for a typically scaled 3d game, but maybe not for 2d on a pixel scale.
+		params_set_pairing_expansion(0.1);
+	}
+
+private:
+	bool node_add_child(uint32_t p_node_id, uint32_t p_child_node_id) {
+		TNode &tnode = _nodes[p_node_id];
+		if (tnode.is_full_of_children()) {
+			return false;
+		}
+
+		tnode.children[tnode.num_children] = p_child_node_id;
+		tnode.num_children += 1;
+
+		// back link in the child to the parent
+		TNode &tnode_child = _nodes[p_child_node_id];
+		tnode_child.parent_id = p_node_id;
+
+		return true;
+	}
+
+	void node_replace_child(uint32_t p_parent_id, uint32_t p_old_child_id, uint32_t p_new_child_id) {
+		TNode &parent = _nodes[p_parent_id];
+		BVH_ASSERT(!parent.is_leaf());
+
+		int child_num = parent.find_child(p_old_child_id);
+		BVH_ASSERT(child_num != -1);
+		parent.children[child_num] = p_new_child_id;
+
+		TNode &new_child = _nodes[p_new_child_id];
+		new_child.parent_id = p_parent_id;
+	}
+
+	void node_remove_child(uint32_t p_parent_id, uint32_t p_child_id, uint32_t p_tree_id, bool p_prevent_sibling = false) {
+		TNode &parent = _nodes[p_parent_id];
+		BVH_ASSERT(!parent.is_leaf());
+
+		int child_num = parent.find_child(p_child_id);
+		BVH_ASSERT(child_num != -1);
+
+		parent.remove_child_internal(child_num);
+
+		// no need to keep back references for children at the moment
+
+		uint32_t sibling_id = 0; // always a node id, as tnode is never a leaf
+		bool sibling_present = false;
+
+		// if there are more children, or this is the root node, don't try and delete
+		if (parent.num_children > 1) {
+			return;
+		}
+
+		// if there is 1 sibling, it can be moved to be a child of the
+		if (parent.num_children == 1) {
+			// else there is now a redundant node with one child, which can be removed
+			sibling_id = parent.children[0];
+			sibling_present = true;
+		}
+
+		// now there may be no children in this node .. in which case it can be deleted
+		// remove node if empty
+		// remove link from parent
+		uint32_t grandparent_id = parent.parent_id;
+
+		// special case for root node
+		if (grandparent_id == BVHCommon::INVALID) {
+			if (sibling_present) {
+				// change the root node
+				change_root_node(sibling_id, p_tree_id);
+
+				// delete the old root node as no longer needed
+				node_free_node_and_leaf(p_parent_id);
+			}
+
+			return;
+		}
+
+		if (sibling_present) {
+			node_replace_child(grandparent_id, p_parent_id, sibling_id);
+		} else {
+			node_remove_child(grandparent_id, p_parent_id, p_tree_id, true);
+		}
+
+		// put the node on the free list to recycle
+		node_free_node_and_leaf(p_parent_id);
+	}
+
+	// A node can either be a node, or a node AND a leaf combo.
+	// Both must be deleted to prevent a leak.
+	void node_free_node_and_leaf(uint32_t p_node_id) {
+		TNode &node = _nodes[p_node_id];
+		if (node.is_leaf()) {
+			int leaf_id = node.get_leaf_id();
+			_leaves.free(leaf_id);
+		}
+
+		_nodes.free(p_node_id);
+	}
+
+	void change_root_node(uint32_t p_new_root_id, uint32_t p_tree_id) {
+		_root_node_id[p_tree_id] = p_new_root_id;
+		TNode &root = _nodes[p_new_root_id];
+
+		// mark no parent
+		root.parent_id = BVHCommon::INVALID;
+	}
+
+	void node_make_leaf(uint32_t p_node_id) {
+		uint32_t child_leaf_id;
+		TLeaf *child_leaf = _leaves.request(child_leaf_id);
+		child_leaf->clear();
+
+		// zero is reserved at startup, to prevent this id being used
+		// (as they are stored as negative values in the node, and zero is already taken)
+		BVH_ASSERT(child_leaf_id != 0);
+
+		TNode &node = _nodes[p_node_id];
+		node.neg_leaf_id = -(int)child_leaf_id;
+	}
+
+	void node_remove_item(uint32_t p_ref_id, uint32_t p_tree_id, BVHABB_CLASS *r_old_aabb = nullptr) {
+		// get the reference
+		ItemRef &ref = _refs[p_ref_id];
+		uint32_t owner_node_id = ref.tnode_id;
+
+		// debug draw special
+		// This may not be needed
+		if (owner_node_id == BVHCommon::INVALID) {
+			return;
+		}
+
+		TNode &tnode = _nodes[owner_node_id];
+		CRASH_COND(!tnode.is_leaf());
+
+		TLeaf &leaf = _node_get_leaf(tnode);
+
+		// if the aabb is not determining the corner size, then there is no need to refit!
+		// (optimization, as merging AABBs takes a lot of time)
+		const BVHABB_CLASS &old_aabb = leaf.get_aabb(ref.item_id);
+
+		// shrink a little to prevent using corner aabbs
+		// in order to miss the corners first we shrink by node_expansion
+		// (which is added to the overall bound of the leaf), then we also
+		// shrink by an epsilon, in order to miss out the very corner aabbs
+		// which are important in determining the bound. Any other aabb
+		// within this can be removed and not affect the overall bound.
+		BVHABB_CLASS node_bound = tnode.aabb;
+		node_bound.expand(-_node_expansion - 0.001f);
+		bool refit = true;
+
+		if (node_bound.is_other_within(old_aabb)) {
+			refit = false;
+		}
+
+		// record the old aabb if required (for incremental remove_and_reinsert)
+		if (r_old_aabb) {
+			*r_old_aabb = old_aabb;
+		}
+
+		leaf.remove_item_unordered(ref.item_id);
+
+		if (leaf.num_items) {
+			// the swapped item has to have its reference changed to, to point to the new item id
+			uint32_t swapped_ref_id = leaf.get_item_ref_id(ref.item_id);
+
+			ItemRef &swapped_ref = _refs[swapped_ref_id];
+
+			swapped_ref.item_id = ref.item_id;
+
+			// only have to refit if it is an edge item
+			// This is a VERY EXPENSIVE STEP
+			// we defer the refit updates until the update function is called once per frame
+			if (refit) {
+				leaf.set_dirty(true);
+			}
+		} else {
+			// remove node if empty
+			// remove link from parent
+			if (tnode.parent_id != BVHCommon::INVALID) {
+				// DANGER .. this can potentially end up with root node with 1 child ...
+				// we don't want this and must check for it
+
+				uint32_t parent_id = tnode.parent_id;
+
+				node_remove_child(parent_id, owner_node_id, p_tree_id);
+				refit_upward(parent_id);
+
+				// put the node on the free list to recycle
+				node_free_node_and_leaf(owner_node_id);
+			}
+
+			// else if no parent, it is the root node. Do not delete
+		}
+
+		ref.tnode_id = BVHCommon::INVALID;
+		ref.item_id = BVHCommon::INVALID; // unset
+	}
+
+	// returns true if needs refit of PARENT tree only, the node itself AABB is calculated
+	// within this routine
+	bool _node_add_item(uint32_t p_node_id, uint32_t p_ref_id, const BVHABB_CLASS &p_aabb) {
+		ItemRef &ref = _refs[p_ref_id];
+		ref.tnode_id = p_node_id;
+
+		TNode &node = _nodes[p_node_id];
+		BVH_ASSERT(node.is_leaf());
+		TLeaf &leaf = _node_get_leaf(node);
+
+		// optimization - we only need to do a refit
+		// if the added item is changing the AABB of the node.
+		// in most cases it won't.
+		bool needs_refit = true;
+
+		// expand bound now
+		BVHABB_CLASS expanded = p_aabb;
+		expanded.expand(_node_expansion);
+
+		// the bound will only be valid if there is an item in there already
+		if (leaf.num_items) {
+			if (node.aabb.is_other_within(expanded)) {
+				// no change to node AABBs
+				needs_refit = false;
+			} else {
+				node.aabb.merge(expanded);
+			}
+		} else {
+			// bound of the node = the new aabb
+			node.aabb = expanded;
+		}
+
+		ref.item_id = leaf.request_item();
+		BVH_ASSERT(ref.item_id != BVHCommon::INVALID);
+
+		// set the aabb of the new item
+		leaf.get_aabb(ref.item_id) = p_aabb;
+
+		// back reference on the item back to the item reference
+		leaf.get_item_ref_id(ref.item_id) = p_ref_id;
+
+		return needs_refit;
+	}
+
+	uint32_t _node_create_another_child(uint32_t p_node_id, const BVHABB_CLASS &p_aabb) {
+		uint32_t child_node_id;
+		TNode *child_node = _nodes.request(child_node_id);
+		child_node->clear();
+
+		// may not be necessary
+		child_node->aabb = p_aabb;
+
+		node_add_child(p_node_id, child_node_id);
+
+		return child_node_id;
+	}
+
+#include "bvh_cull.inc"
+#include "bvh_debug.inc"
+#include "bvh_integrity.inc"
+#include "bvh_logic.inc"
+#include "bvh_misc.inc"
+#include "bvh_public.inc"
+#include "bvh_refit.inc"
+#include "bvh_split.inc"
+};
+
+#undef VERBOSE_PRINT

core/math/color.cpp

@@ -0,0 +1,532 @@
+/**************************************************************************/
+/*  color.cpp                                                             */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "color.h"
+
+#include "color_names.inc"
+#include "core/math/math_funcs.h"
+#include "core/string/ustring.h"
+#include "core/templates/hash_map.h"
+
+#include "thirdparty/misc/ok_color.h"
+
+uint32_t Color::to_argb32() const {
+	uint32_t c = (uint8_t)Math::round(a * 255.0f);
+	c <<= 8;
+	c |= (uint8_t)Math::round(r * 255.0f);
+	c <<= 8;
+	c |= (uint8_t)Math::round(g * 255.0f);
+	c <<= 8;
+	c |= (uint8_t)Math::round(b * 255.0f);
+
+	return c;
+}
+
+uint32_t Color::to_abgr32() const {
+	uint32_t c = (uint8_t)Math::round(a * 255.0f);
+	c <<= 8;
+	c |= (uint8_t)Math::round(b * 255.0f);
+	c <<= 8;
+	c |= (uint8_t)Math::round(g * 255.0f);
+	c <<= 8;
+	c |= (uint8_t)Math::round(r * 255.0f);
+
+	return c;
+}
+
+uint32_t Color::to_rgba32() const {
+	uint32_t c = (uint8_t)Math::round(r * 255.0f);
+	c <<= 8;
+	c |= (uint8_t)Math::round(g * 255.0f);
+	c <<= 8;
+	c |= (uint8_t)Math::round(b * 255.0f);
+	c <<= 8;
+	c |= (uint8_t)Math::round(a * 255.0f);
+
+	return c;
+}
+
+uint64_t Color::to_abgr64() const {
+	uint64_t c = (uint16_t)Math::round(a * 65535.0f);
+	c <<= 16;
+	c |= (uint16_t)Math::round(b * 65535.0f);
+	c <<= 16;
+	c |= (uint16_t)Math::round(g * 65535.0f);
+	c <<= 16;
+	c |= (uint16_t)Math::round(r * 65535.0f);
+
+	return c;
+}
+
+uint64_t Color::to_argb64() const {
+	uint64_t c = (uint16_t)Math::round(a * 65535.0f);
+	c <<= 16;
+	c |= (uint16_t)Math::round(r * 65535.0f);
+	c <<= 16;
+	c |= (uint16_t)Math::round(g * 65535.0f);
+	c <<= 16;
+	c |= (uint16_t)Math::round(b * 65535.0f);
+
+	return c;
+}
+
+uint64_t Color::to_rgba64() const {
+	uint64_t c = (uint16_t)Math::round(r * 65535.0f);
+	c <<= 16;
+	c |= (uint16_t)Math::round(g * 65535.0f);
+	c <<= 16;
+	c |= (uint16_t)Math::round(b * 65535.0f);
+	c <<= 16;
+	c |= (uint16_t)Math::round(a * 65535.0f);
+
+	return c;
+}
+
+void _append_hex(float p_val, char32_t *string) {
+	int v = Math::round(p_val * 255.0f);
+	v = CLAMP(v, 0, 255);
+
+	string[0] = hex_char_table_lower[(v >> 4) & 0xF];
+	string[1] = hex_char_table_lower[v & 0xF];
+}
+
+String Color::to_html(bool p_alpha) const {
+	String txt;
+	txt.resize_uninitialized(p_alpha ? 9 : 7);
+	char32_t *ptr = txt.ptrw();
+
+	_append_hex(r, ptr + 0);
+	_append_hex(g, ptr + 2);
+	_append_hex(b, ptr + 4);
+	if (p_alpha) {
+		_append_hex(a, ptr + 6);
+	}
+	ptr[txt.size() - 1] = '\0';
+
+	return txt;
+}
+
+float Color::get_h() const {
+	float min = MIN(r, g);
+	min = MIN(min, b);
+	float max = MAX(r, g);
+	max = MAX(max, b);
+
+	float delta = max - min;
+
+	if (delta == 0.0f) {
+		return 0.0f;
+	}
+
+	float h;
+	if (r == max) {
+		h = (g - b) / delta; // between yellow & magenta
+	} else if (g == max) {
+		h = 2 + (b - r) / delta; // between cyan & yellow
+	} else {
+		h = 4 + (r - g) / delta; // between magenta & cyan
+	}
+
+	h /= 6.0f;
+	if (h < 0.0f) {
+		h += 1.0f;
+	}
+
+	return h;
+}
+
+float Color::get_s() const {
+	float min = MIN(r, g);
+	min = MIN(min, b);
+	float max = MAX(r, g);
+	max = MAX(max, b);
+
+	float delta = max - min;
+
+	return (max != 0.0f) ? (delta / max) : 0.0f;
+}
+
+float Color::get_v() const {
+	float max = MAX(r, g);
+	max = MAX(max, b);
+	return max;
+}
+
+void Color::set_hsv(float p_h, float p_s, float p_v, float p_alpha) {
+	int i;
+	float f, p, q, t;
+	a = p_alpha;
+
+	if (p_s == 0.0f) {
+		// Achromatic (gray)
+		r = g = b = p_v;
+		return;
+	}
+
+	p_h *= 6.0f;
+	p_h = Math::fmod(p_h, 6);
+	i = Math::floor(p_h);
+
+	f = p_h - i;
+	p = p_v * (1.0f - p_s);
+	q = p_v * (1.0f - p_s * f);
+	t = p_v * (1.0f - p_s * (1.0f - f));
+
+	switch (i) {
+		case 0: // Red is the dominant color
+			r = p_v;
+			g = t;
+			b = p;
+			break;
+		case 1: // Green is the dominant color
+			r = q;
+			g = p_v;
+			b = p;
+			break;
+		case 2:
+			r = p;
+			g = p_v;
+			b = t;
+			break;
+		case 3: // Blue is the dominant color
+			r = p;
+			g = q;
+			b = p_v;
+			break;
+		case 4:
+			r = t;
+			g = p;
+			b = p_v;
+			break;
+		default: // (5) Red is the dominant color
+			r = p_v;
+			g = p;
+			b = q;
+			break;
+	}
+}
+
+void Color::set_ok_hsl(float p_h, float p_s, float p_l, float p_alpha) {
+	ok_color::HSL hsl;
+	hsl.h = p_h;
+	hsl.s = p_s;
+	hsl.l = p_l;
+	ok_color::RGB rgb = ok_color::okhsl_to_srgb(hsl);
+	Color c = Color(rgb.r, rgb.g, rgb.b, p_alpha).clamp();
+	r = c.r;
+	g = c.g;
+	b = c.b;
+	a = c.a;
+}
+
+void Color::set_ok_hsv(float p_h, float p_s, float p_v, float p_alpha) {
+	ok_color::HSV hsv;
+	hsv.h = p_h;
+	hsv.s = p_s;
+	hsv.v = p_v;
+	ok_color::RGB rgb = ok_color::okhsv_to_srgb(hsv);
+	Color c = Color(rgb.r, rgb.g, rgb.b, p_alpha).clamp();
+	r = c.r;
+	g = c.g;
+	b = c.b;
+	a = c.a;
+}
+
+bool Color::is_equal_approx(const Color &p_color) const {
+	return Math::is_equal_approx(r, p_color.r) && Math::is_equal_approx(g, p_color.g) && Math::is_equal_approx(b, p_color.b) && Math::is_equal_approx(a, p_color.a);
+}
+
+bool Color::is_same(const Color &p_color) const {
+	return Math::is_same(r, p_color.r) && Math::is_same(g, p_color.g) && Math::is_same(b, p_color.b) && Math::is_same(a, p_color.a);
+}
+
+Color Color::clamp(const Color &p_min, const Color &p_max) const {
+	return Color(
+			CLAMP(r, p_min.r, p_max.r),
+			CLAMP(g, p_min.g, p_max.g),
+			CLAMP(b, p_min.b, p_max.b),
+			CLAMP(a, p_min.a, p_max.a));
+}
+
+void Color::invert() {
+	r = 1.0f - r;
+	g = 1.0f - g;
+	b = 1.0f - b;
+}
+
+Color Color::hex(uint32_t p_hex) {
+	float a = (p_hex & 0xFF) / 255.0f;
+	p_hex >>= 8;
+	float b = (p_hex & 0xFF) / 255.0f;
+	p_hex >>= 8;
+	float g = (p_hex & 0xFF) / 255.0f;
+	p_hex >>= 8;
+	float r = (p_hex & 0xFF) / 255.0f;
+
+	return Color(r, g, b, a);
+}
+
+Color Color::hex64(uint64_t p_hex) {
+	float a = (p_hex & 0xFFFF) / 65535.0f;
+	p_hex >>= 16;
+	float b = (p_hex & 0xFFFF) / 65535.0f;
+	p_hex >>= 16;
+	float g = (p_hex & 0xFFFF) / 65535.0f;
+	p_hex >>= 16;
+	float r = (p_hex & 0xFFFF) / 65535.0f;
+
+	return Color(r, g, b, a);
+}
+
+static int _parse_col4(const String &p_str, int p_ofs) {
+	char character = p_str[p_ofs];
+
+	if (character >= '0' && character <= '9') {
+		return character - '0';
+	} else if (character >= 'a' && character <= 'f') {
+		return character + (10 - 'a');
+	} else if (character >= 'A' && character <= 'F') {
+		return character + (10 - 'A');
+	}
+	return -1;
+}
+
+static int _parse_col8(const String &p_str, int p_ofs) {
+	return _parse_col4(p_str, p_ofs) * 16 + _parse_col4(p_str, p_ofs + 1);
+}
+
+Color Color::inverted() const {
+	Color c = *this;
+	c.invert();
+	return c;
+}
+
+Color Color::html(const String &p_rgba) {
+	if (p_rgba.is_empty()) {
+		return Color();
+	}
+
+	const int current_pos = (p_rgba[0] == '#') ? 1 : 0;
+	const int num_of_digits = p_rgba.length() - current_pos;
+
+	float r, g, b, a = 1.0f;
+
+	if (num_of_digits == 3) {
+		// #rgb
+		r = _parse_col4(p_rgba, current_pos) / 15.0f;
+		g = _parse_col4(p_rgba, current_pos + 1) / 15.0f;
+		b = _parse_col4(p_rgba, current_pos + 2) / 15.0f;
+	} else if (num_of_digits == 4) {
+		r = _parse_col4(p_rgba, current_pos) / 15.0f;
+		g = _parse_col4(p_rgba, current_pos + 1) / 15.0f;
+		b = _parse_col4(p_rgba, current_pos + 2) / 15.0f;
+		a = _parse_col4(p_rgba, current_pos + 3) / 15.0f;
+	} else if (num_of_digits == 6) {
+		r = _parse_col8(p_rgba, current_pos) / 255.0f;
+		g = _parse_col8(p_rgba, current_pos + 2) / 255.0f;
+		b = _parse_col8(p_rgba, current_pos + 4) / 255.0f;
+	} else if (num_of_digits == 8) {
+		r = _parse_col8(p_rgba, current_pos) / 255.0f;
+		g = _parse_col8(p_rgba, current_pos + 2) / 255.0f;
+		b = _parse_col8(p_rgba, current_pos + 4) / 255.0f;
+		a = _parse_col8(p_rgba, current_pos + 6) / 255.0f;
+	} else {
+		ERR_FAIL_V_MSG(Color(), "Invalid color code: " + p_rgba + ".");
+	}
+
+	ERR_FAIL_COND_V_MSG(r < 0.0f, Color(), "Invalid color code: " + p_rgba + ".");
+	ERR_FAIL_COND_V_MSG(g < 0.0f, Color(), "Invalid color code: " + p_rgba + ".");
+	ERR_FAIL_COND_V_MSG(b < 0.0f, Color(), "Invalid color code: " + p_rgba + ".");
+	ERR_FAIL_COND_V_MSG(a < 0.0f, Color(), "Invalid color code: " + p_rgba + ".");
+
+	return Color(r, g, b, a);
+}
+
+bool Color::html_is_valid(const String &p_color) {
+	String color = p_color;
+
+	if (color.is_empty()) {
+		return false;
+	}
+
+	const int current_pos = (color[0] == '#') ? 1 : 0;
+	const int len = color.length();
+	const int num_of_digits = len - current_pos;
+	if (!(num_of_digits == 3 || num_of_digits == 4 || num_of_digits == 6 || num_of_digits == 8)) {
+		return false;
+	}
+
+	// Check if each hex digit is valid.
+	for (int i = current_pos; i < len; i++) {
+		if (!is_hex_digit(p_color[i])) {
+			return false;
+		}
+	}
+
+	return true;
+}
+
+Color Color::named(const String &p_name) {
+	int idx = find_named_color(p_name);
+	if (idx == -1) {
+		ERR_FAIL_V_MSG(Color(), "Invalid color name: " + p_name + ".");
+	}
+	return named_colors[idx].color;
+}
+
+Color Color::named(const String &p_name, const Color &p_default) {
+	int idx = find_named_color(p_name);
+	if (idx == -1) {
+		return p_default;
+	}
+	return named_colors[idx].color;
+}
+
+int Color::find_named_color(const String &p_name) {
+	String name = p_name;
+	// Normalize name.
+	name = name.remove_chars(" -_'.");
+	name = name.to_upper();
+
+	static HashMap<String, int> named_colors_hashmap;
+	if (unlikely(named_colors_hashmap.is_empty())) {
+		const int named_color_count = get_named_color_count();
+		for (int i = 0; i < named_color_count; i++) {
+			named_colors_hashmap[String(named_colors[i].name).remove_char('_')] = i;
+		}
+	}
+
+	const HashMap<String, int>::ConstIterator E = named_colors_hashmap.find(name);
+	if (E) {
+		return E->value;
+	}
+
+	return -1;
+}
+
+int Color::get_named_color_count() {
+	return std::size(named_colors);
+}
+
+String Color::get_named_color_name(int p_idx) {
+	ERR_FAIL_INDEX_V(p_idx, get_named_color_count(), "");
+	return named_colors[p_idx].name;
+}
+
+Color Color::get_named_color(int p_idx) {
+	ERR_FAIL_INDEX_V(p_idx, get_named_color_count(), Color());
+	return named_colors[p_idx].color;
+}
+
+// For a version that errors on invalid values instead of returning
+// a default color, use the Color(String) constructor instead.
+Color Color::from_string(const String &p_string, const Color &p_default) {
+	if (html_is_valid(p_string)) {
+		return html(p_string);
+	} else {
+		return named(p_string, p_default);
+	}
+}
+
+Color Color::from_hsv(float p_h, float p_s, float p_v, float p_alpha) {
+	Color c;
+	c.set_hsv(p_h, p_s, p_v, p_alpha);
+	return c;
+}
+
+Color Color::from_rgbe9995(uint32_t p_rgbe) {
+	float r = p_rgbe & 0x1ff;
+	float g = (p_rgbe >> 9) & 0x1ff;
+	float b = (p_rgbe >> 18) & 0x1ff;
+	float e = (p_rgbe >> 27);
+	float m = Math::pow(2.0f, e - 15.0f - 9.0f);
+
+	float rd = r * m;
+	float gd = g * m;
+	float bd = b * m;
+
+	return Color(rd, gd, bd, 1.0f);
+}
+
+Color Color::from_rgba8(int64_t p_r8, int64_t p_g8, int64_t p_b8, int64_t p_a8) {
+	return Color(p_r8 / 255.0f, p_g8 / 255.0f, p_b8 / 255.0f, p_a8 / 255.0f);
+}
+
+Color::operator String() const {
+	return "(" + String::num(r, 4) + ", " + String::num(g, 4) + ", " + String::num(b, 4) + ", " + String::num(a, 4) + ")";
+}
+
+Color Color::from_ok_hsl(float p_h, float p_s, float p_l, float p_alpha) {
+	Color c;
+	c.set_ok_hsl(p_h, p_s, p_l, p_alpha);
+	return c;
+}
+
+Color Color::from_ok_hsv(float p_h, float p_s, float p_l, float p_alpha) {
+	Color c;
+	c.set_ok_hsv(p_h, p_s, p_l, p_alpha);
+	return c;
+}
+
+float Color::get_ok_hsl_h() const {
+	ok_color::RGB rgb;
+	rgb.r = r;
+	rgb.g = g;
+	rgb.b = b;
+	ok_color::HSL ok_hsl = ok_color::srgb_to_okhsl(rgb);
+	if (Math::is_nan(ok_hsl.h)) {
+		return 0.0f;
+	}
+	return CLAMP(ok_hsl.h, 0.0f, 1.0f);
+}
+
+float Color::get_ok_hsl_s() const {
+	ok_color::RGB rgb;
+	rgb.r = r;
+	rgb.g = g;
+	rgb.b = b;
+	ok_color::HSL ok_hsl = ok_color::srgb_to_okhsl(rgb);
+	if (Math::is_nan(ok_hsl.s)) {
+		return 0.0f;
+	}
+	return CLAMP(ok_hsl.s, 0.0f, 1.0f);
+}
+
+float Color::get_ok_hsl_l() const {
+	ok_color::RGB rgb;
+	rgb.r = r;
+	rgb.g = g;
+	rgb.b = b;
+	ok_color::HSL ok_hsl = ok_color::srgb_to_okhsl(rgb);
+	if (Math::is_nan(ok_hsl.l)) {
+		return 0.0f;
+	}
+	return CLAMP(ok_hsl.l, 0.0f, 1.0f);
+}

core/math/color.h

@@ -0,0 +1,396 @@
+/**************************************************************************/
+/*  color.h                                                               */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "core/math/math_funcs.h"
+
+class String;
+
+struct [[nodiscard]] Color {
+	union {
+		// NOLINTBEGIN(modernize-use-default-member-init)
+		struct {
+			float r;
+			float g;
+			float b;
+			float a;
+		};
+		float components[4] = { 0, 0, 0, 1.0 };
+		// NOLINTEND(modernize-use-default-member-init)
+	};
+
+	uint32_t to_rgba32() const;
+	uint32_t to_argb32() const;
+	uint32_t to_abgr32() const;
+	uint64_t to_rgba64() const;
+	uint64_t to_argb64() const;
+	uint64_t to_abgr64() const;
+	String to_html(bool p_alpha = true) const;
+	float get_h() const;
+	float get_s() const;
+	float get_v() const;
+	void set_hsv(float p_h, float p_s, float p_v, float p_alpha = 1.0f);
+	float get_ok_hsl_h() const;
+	float get_ok_hsl_s() const;
+	float get_ok_hsl_l() const;
+	void set_ok_hsl(float p_h, float p_s, float p_l, float p_alpha = 1.0f);
+	void set_ok_hsv(float p_h, float p_s, float p_v, float p_alpha = 1.0f);
+
+	_FORCE_INLINE_ float &operator[](int p_idx) {
+		return components[p_idx];
+	}
+	_FORCE_INLINE_ const float &operator[](int p_idx) const {
+		return components[p_idx];
+	}
+
+	constexpr bool operator==(const Color &p_color) const {
+		return (r == p_color.r && g == p_color.g && b == p_color.b && a == p_color.a);
+	}
+	constexpr bool operator!=(const Color &p_color) const {
+		return (r != p_color.r || g != p_color.g || b != p_color.b || a != p_color.a);
+	}
+
+	constexpr Color operator+(const Color &p_color) const;
+	constexpr void operator+=(const Color &p_color);
+
+	constexpr Color operator-() const;
+	constexpr Color operator-(const Color &p_color) const;
+	constexpr void operator-=(const Color &p_color);
+
+	constexpr Color operator*(const Color &p_color) const;
+	constexpr Color operator*(float p_scalar) const;
+	constexpr void operator*=(const Color &p_color);
+	constexpr void operator*=(float p_scalar);
+
+	constexpr Color operator/(const Color &p_color) const;
+	constexpr Color operator/(float p_scalar) const;
+	constexpr void operator/=(const Color &p_color);
+	constexpr void operator/=(float p_scalar);
+
+	bool is_equal_approx(const Color &p_color) const;
+	bool is_same(const Color &p_color) const;
+
+	Color clamp(const Color &p_min = Color(0, 0, 0, 0), const Color &p_max = Color(1, 1, 1, 1)) const;
+	void invert();
+	Color inverted() const;
+
+	_FORCE_INLINE_ float get_luminance() const {
+		return 0.2126f * r + 0.7152f * g + 0.0722f * b;
+	}
+
+	_FORCE_INLINE_ Color lerp(const Color &p_to, float p_weight) const {
+		Color res = *this;
+		res.r = Math::lerp(res.r, p_to.r, p_weight);
+		res.g = Math::lerp(res.g, p_to.g, p_weight);
+		res.b = Math::lerp(res.b, p_to.b, p_weight);
+		res.a = Math::lerp(res.a, p_to.a, p_weight);
+		return res;
+	}
+
+	_FORCE_INLINE_ Color darkened(float p_amount) const {
+		Color res = *this;
+		res.r = res.r * (1.0f - p_amount);
+		res.g = res.g * (1.0f - p_amount);
+		res.b = res.b * (1.0f - p_amount);
+		return res;
+	}
+
+	_FORCE_INLINE_ Color lightened(float p_amount) const {
+		Color res = *this;
+		res.r = res.r + (1.0f - res.r) * p_amount;
+		res.g = res.g + (1.0f - res.g) * p_amount;
+		res.b = res.b + (1.0f - res.b) * p_amount;
+		return res;
+	}
+
+	_FORCE_INLINE_ uint32_t to_rgbe9995() const {
+		// https://github.com/microsoft/DirectX-Graphics-Samples/blob/v10.0.19041.0/MiniEngine/Core/Color.cpp
+		static const float kMaxVal = float(0x1FF << 7);
+		static const float kMinVal = float(1.f / (1 << 16));
+
+		// Clamp RGB to [0, 1.FF*2^16]
+		const float _r = CLAMP(r, 0.0f, kMaxVal);
+		const float _g = CLAMP(g, 0.0f, kMaxVal);
+		const float _b = CLAMP(b, 0.0f, kMaxVal);
+
+		// Compute the maximum channel, no less than 1.0*2^-15
+		const float MaxChannel = MAX(MAX(_r, _g), MAX(_b, kMinVal));
+
+		// Take the exponent of the maximum channel (rounding up the 9th bit) and
+		// add 15 to it.  When added to the channels, it causes the implicit '1.0'
+		// bit and the first 8 mantissa bits to be shifted down to the low 9 bits
+		// of the mantissa, rounding the truncated bits.
+		union {
+			float f;
+			uint32_t i;
+		} R, G, B, E;
+
+		E.f = MaxChannel;
+		E.i += 0x07804000; // Add 15 to the exponent and 0x4000 to the mantissa
+		E.i &= 0x7F800000; // Zero the mantissa
+
+		// This shifts the 9-bit values we need into the lowest bits, rounding as
+		// needed. Note that if the channel has a smaller exponent than the max
+		// channel, it will shift even more.  This is intentional.
+		R.f = _r + E.f;
+		G.f = _g + E.f;
+		B.f = _b + E.f;
+
+		// Convert the Bias to the correct exponent in the upper 5 bits.
+		E.i <<= 4;
+		E.i += 0x10000000;
+
+		// Combine the fields. RGB floats have unwanted data in the upper 9
+		// bits. Only red needs to mask them off because green and blue shift
+		// it out to the left.
+		return E.i | (B.i << 18U) | (G.i << 9U) | (R.i & 511U);
+	}
+
+	_FORCE_INLINE_ Color blend(const Color &p_over) const {
+		Color res;
+		float sa = 1.0f - p_over.a;
+		res.a = a * sa + p_over.a;
+		if (res.a == 0) {
+			return Color(0, 0, 0, 0);
+		} else {
+			res.r = (r * a * sa + p_over.r * p_over.a) / res.a;
+			res.g = (g * a * sa + p_over.g * p_over.a) / res.a;
+			res.b = (b * a * sa + p_over.b * p_over.a) / res.a;
+		}
+		return res;
+	}
+
+	_FORCE_INLINE_ Color srgb_to_linear() const {
+		return Color(
+				r < 0.04045f ? r * (1.0f / 12.92f) : Math::pow(float((r + 0.055) * (1.0 / (1.0 + 0.055))), 2.4f),
+				g < 0.04045f ? g * (1.0f / 12.92f) : Math::pow(float((g + 0.055) * (1.0 / (1.0 + 0.055))), 2.4f),
+				b < 0.04045f ? b * (1.0f / 12.92f) : Math::pow(float((b + 0.055) * (1.0 / (1.0 + 0.055))), 2.4f),
+				a);
+	}
+	_FORCE_INLINE_ Color linear_to_srgb() const {
+		return Color(
+				r < 0.0031308f ? 12.92f * r : (1.0 + 0.055) * Math::pow(r, 1.0f / 2.4f) - 0.055,
+				g < 0.0031308f ? 12.92f * g : (1.0 + 0.055) * Math::pow(g, 1.0f / 2.4f) - 0.055,
+				b < 0.0031308f ? 12.92f * b : (1.0 + 0.055) * Math::pow(b, 1.0f / 2.4f) - 0.055, a);
+	}
+
+	static Color hex(uint32_t p_hex);
+	static Color hex64(uint64_t p_hex);
+	static Color html(const String &p_rgba);
+	static bool html_is_valid(const String &p_color);
+	static Color named(const String &p_name);
+	static Color named(const String &p_name, const Color &p_default);
+	static int find_named_color(const String &p_name);
+	static int get_named_color_count();
+	static String get_named_color_name(int p_idx);
+	static Color get_named_color(int p_idx);
+	static Color from_string(const String &p_string, const Color &p_default);
+	static Color from_hsv(float p_h, float p_s, float p_v, float p_alpha = 1.0f);
+	static Color from_ok_hsl(float p_h, float p_s, float p_l, float p_alpha = 1.0f);
+	static Color from_ok_hsv(float p_h, float p_s, float p_l, float p_alpha = 1.0f);
+	static Color from_rgbe9995(uint32_t p_rgbe);
+	static Color from_rgba8(int64_t p_r8, int64_t p_g8, int64_t p_b8, int64_t p_a8 = 255);
+
+	constexpr bool operator<(const Color &p_color) const; // Used in set keys.
+	explicit operator String() const;
+
+	// For the binder.
+	_FORCE_INLINE_ void set_r8(int32_t r8) { r = (CLAMP(r8, 0, 255) / 255.0f); }
+	_FORCE_INLINE_ int32_t get_r8() const { return int32_t(CLAMP(Math::round(r * 255.0f), 0.0f, 255.0f)); }
+	_FORCE_INLINE_ void set_g8(int32_t g8) { g = (CLAMP(g8, 0, 255) / 255.0f); }
+	_FORCE_INLINE_ int32_t get_g8() const { return int32_t(CLAMP(Math::round(g * 255.0f), 0.0f, 255.0f)); }
+	_FORCE_INLINE_ void set_b8(int32_t b8) { b = (CLAMP(b8, 0, 255) / 255.0f); }
+	_FORCE_INLINE_ int32_t get_b8() const { return int32_t(CLAMP(Math::round(b * 255.0f), 0.0f, 255.0f)); }
+	_FORCE_INLINE_ void set_a8(int32_t a8) { a = (CLAMP(a8, 0, 255) / 255.0f); }
+	_FORCE_INLINE_ int32_t get_a8() const { return int32_t(CLAMP(Math::round(a * 255.0f), 0.0f, 255.0f)); }
+
+	_FORCE_INLINE_ void set_h(float p_h) { set_hsv(p_h, get_s(), get_v(), a); }
+	_FORCE_INLINE_ void set_s(float p_s) { set_hsv(get_h(), p_s, get_v(), a); }
+	_FORCE_INLINE_ void set_v(float p_v) { set_hsv(get_h(), get_s(), p_v, a); }
+	_FORCE_INLINE_ void set_ok_hsl_h(float p_h) { set_ok_hsl(p_h, get_ok_hsl_s(), get_ok_hsl_l(), a); }
+	_FORCE_INLINE_ void set_ok_hsl_s(float p_s) { set_ok_hsl(get_ok_hsl_h(), p_s, get_ok_hsl_l(), a); }
+	_FORCE_INLINE_ void set_ok_hsl_l(float p_l) { set_ok_hsl(get_ok_hsl_h(), get_ok_hsl_s(), p_l, a); }
+
+	constexpr Color() :
+			r(0), g(0), b(0), a(1) {}
+
+	/**
+	 * RGBA construct parameters.
+	 * Alpha is not optional as otherwise we can't bind the RGB version for scripting.
+	 */
+	constexpr Color(float p_r, float p_g, float p_b, float p_a) :
+			r(p_r), g(p_g), b(p_b), a(p_a) {}
+
+	/**
+	 * RGB construct parameters.
+	 */
+	constexpr Color(float p_r, float p_g, float p_b) :
+			r(p_r), g(p_g), b(p_b), a(1) {}
+
+	/**
+	 * Construct a Color from another Color, but with the specified alpha value.
+	 */
+	constexpr Color(const Color &p_c, float p_a) :
+			r(p_c.r), g(p_c.g), b(p_c.b), a(p_a) {}
+
+	// NOLINTBEGIN(cppcoreguidelines-pro-type-member-init)
+	Color(const String &p_code) {
+		if (html_is_valid(p_code)) {
+			*this = html(p_code);
+		} else {
+			*this = named(p_code);
+		}
+	}
+
+	Color(const String &p_code, float p_a) {
+		*this = Color(p_code);
+		a = p_a;
+	}
+	// NOLINTEND(cppcoreguidelines-pro-type-member-init)
+};
+
+constexpr Color Color::operator+(const Color &p_color) const {
+	return Color(
+			r + p_color.r,
+			g + p_color.g,
+			b + p_color.b,
+			a + p_color.a);
+}
+
+constexpr void Color::operator+=(const Color &p_color) {
+	r = r + p_color.r;
+	g = g + p_color.g;
+	b = b + p_color.b;
+	a = a + p_color.a;
+}
+
+constexpr Color Color::operator-(const Color &p_color) const {
+	return Color(
+			r - p_color.r,
+			g - p_color.g,
+			b - p_color.b,
+			a - p_color.a);
+}
+
+constexpr void Color::operator-=(const Color &p_color) {
+	r = r - p_color.r;
+	g = g - p_color.g;
+	b = b - p_color.b;
+	a = a - p_color.a;
+}
+
+constexpr Color Color::operator*(const Color &p_color) const {
+	return Color(
+			r * p_color.r,
+			g * p_color.g,
+			b * p_color.b,
+			a * p_color.a);
+}
+
+constexpr Color Color::operator*(float p_scalar) const {
+	return Color(
+			r * p_scalar,
+			g * p_scalar,
+			b * p_scalar,
+			a * p_scalar);
+}
+
+constexpr void Color::operator*=(const Color &p_color) {
+	r = r * p_color.r;
+	g = g * p_color.g;
+	b = b * p_color.b;
+	a = a * p_color.a;
+}
+
+constexpr void Color::operator*=(float p_scalar) {
+	r = r * p_scalar;
+	g = g * p_scalar;
+	b = b * p_scalar;
+	a = a * p_scalar;
+}
+
+constexpr Color Color::operator/(const Color &p_color) const {
+	return Color(
+			r / p_color.r,
+			g / p_color.g,
+			b / p_color.b,
+			a / p_color.a);
+}
+
+constexpr Color Color::operator/(float p_scalar) const {
+	return Color(
+			r / p_scalar,
+			g / p_scalar,
+			b / p_scalar,
+			a / p_scalar);
+}
+
+constexpr void Color::operator/=(const Color &p_color) {
+	r = r / p_color.r;
+	g = g / p_color.g;
+	b = b / p_color.b;
+	a = a / p_color.a;
+}
+
+constexpr void Color::operator/=(float p_scalar) {
+	r = r / p_scalar;
+	g = g / p_scalar;
+	b = b / p_scalar;
+	a = a / p_scalar;
+}
+
+constexpr Color Color::operator-() const {
+	return Color(
+			1.0f - r,
+			1.0f - g,
+			1.0f - b,
+			1.0f - a);
+}
+
+constexpr bool Color::operator<(const Color &p_color) const {
+	if (r == p_color.r) {
+		if (g == p_color.g) {
+			if (b == p_color.b) {
+				return (a < p_color.a);
+			} else {
+				return (b < p_color.b);
+			}
+		} else {
+			return g < p_color.g;
+		}
+	} else {
+		return r < p_color.r;
+	}
+}
+
+constexpr Color operator*(float p_scalar, const Color &p_color) {
+	return p_color * p_scalar;
+}

core/math/color_names.inc

@@ -0,0 +1,196 @@
+/**************************************************************************/
+/*  color_names.inc                                                       */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#pragma once
+
+// Names from https://en.wikipedia.org/wiki/X11_color_names
+
+// So this in a way that does not require memory allocation
+// the old way leaked memory
+// this is not used as often as for more performance to make sense
+
+#include "core/math/color.h"
+
+struct NamedColor {
+	const char *name;
+	Color color;
+};
+
+// NOTE: This data is duplicated in the file:
+// modules/mono/glue/GodotSharp/GodotSharp/Core/Colors.cs
+
+static NamedColor named_colors[] = {
+	{ "ALICE_BLUE", Color::hex(0xF0F8FFFF) },
+	{ "ANTIQUE_WHITE", Color::hex(0xFAEBD7FF) },
+	{ "AQUA", Color::hex(0x00FFFFFF) },
+	{ "AQUAMARINE", Color::hex(0x7FFFD4FF) },
+	{ "AZURE", Color::hex(0xF0FFFFFF) },
+	{ "BEIGE", Color::hex(0xF5F5DCFF) },
+	{ "BISQUE", Color::hex(0xFFE4C4FF) },
+	{ "BLACK", Color::hex(0x000000FF) },
+	{ "BLANCHED_ALMOND", Color::hex(0xFFEBCDFF) },
+	{ "BLUE", Color::hex(0x0000FFFF) },
+	{ "BLUE_VIOLET", Color::hex(0x8A2BE2FF) },
+	{ "BROWN", Color::hex(0xA52A2AFF) },
+	{ "BURLYWOOD", Color::hex(0xDEB887FF) },
+	{ "CADET_BLUE", Color::hex(0x5F9EA0FF) },
+	{ "CHARTREUSE", Color::hex(0x7FFF00FF) },
+	{ "CHOCOLATE", Color::hex(0xD2691EFF) },
+	{ "CORAL", Color::hex(0xFF7F50FF) },
+	{ "CORNFLOWER_BLUE", Color::hex(0x6495EDFF) },
+	{ "CORNSILK", Color::hex(0xFFF8DCFF) },
+	{ "CRIMSON", Color::hex(0xDC143CFF) },
+	{ "CYAN", Color::hex(0x00FFFFFF) },
+	{ "DARK_BLUE", Color::hex(0x00008BFF) },
+	{ "DARK_CYAN", Color::hex(0x008B8BFF) },
+	{ "DARK_GOLDENROD", Color::hex(0xB8860BFF) },
+	{ "DARK_GRAY", Color::hex(0xA9A9A9FF) },
+	{ "DARK_GREEN", Color::hex(0x006400FF) },
+	{ "DARK_KHAKI", Color::hex(0xBDB76BFF) },
+	{ "DARK_MAGENTA", Color::hex(0x8B008BFF) },
+	{ "DARK_OLIVE_GREEN", Color::hex(0x556B2FFF) },
+	{ "DARK_ORANGE", Color::hex(0xFF8C00FF) },
+	{ "DARK_ORCHID", Color::hex(0x9932CCFF) },
+	{ "DARK_RED", Color::hex(0x8B0000FF) },
+	{ "DARK_SALMON", Color::hex(0xE9967AFF) },
+	{ "DARK_SEA_GREEN", Color::hex(0x8FBC8FFF) },
+	{ "DARK_SLATE_BLUE", Color::hex(0x483D8BFF) },
+	{ "DARK_SLATE_GRAY", Color::hex(0x2F4F4FFF) },
+	{ "DARK_TURQUOISE", Color::hex(0x00CED1FF) },
+	{ "DARK_VIOLET", Color::hex(0x9400D3FF) },
+	{ "DEEP_PINK", Color::hex(0xFF1493FF) },
+	{ "DEEP_SKY_BLUE", Color::hex(0x00BFFFFF) },
+	{ "DIM_GRAY", Color::hex(0x696969FF) },
+	{ "DODGER_BLUE", Color::hex(0x1E90FFFF) },
+	{ "FIREBRICK", Color::hex(0xB22222FF) },
+	{ "FLORAL_WHITE", Color::hex(0xFFFAF0FF) },
+	{ "FOREST_GREEN", Color::hex(0x228B22FF) },
+	{ "FUCHSIA", Color::hex(0xFF00FFFF) },
+	{ "GAINSBORO", Color::hex(0xDCDCDCFF) },
+	{ "GHOST_WHITE", Color::hex(0xF8F8FFFF) },
+	{ "GOLD", Color::hex(0xFFD700FF) },
+	{ "GOLDENROD", Color::hex(0xDAA520FF) },
+	{ "GRAY", Color::hex(0xBEBEBEFF) },
+	{ "GREEN", Color::hex(0x00FF00FF) },
+	{ "GREEN_YELLOW", Color::hex(0xADFF2FFF) },
+	{ "HONEYDEW", Color::hex(0xF0FFF0FF) },
+	{ "HOT_PINK", Color::hex(0xFF69B4FF) },
+	{ "INDIAN_RED", Color::hex(0xCD5C5CFF) },
+	{ "INDIGO", Color::hex(0x4B0082FF) },
+	{ "IVORY", Color::hex(0xFFFFF0FF) },
+	{ "KHAKI", Color::hex(0xF0E68CFF) },
+	{ "LAVENDER", Color::hex(0xE6E6FAFF) },
+	{ "LAVENDER_BLUSH", Color::hex(0xFFF0F5FF) },
+	{ "LAWN_GREEN", Color::hex(0x7CFC00FF) },
+	{ "LEMON_CHIFFON", Color::hex(0xFFFACDFF) },
+	{ "LIGHT_BLUE", Color::hex(0xADD8E6FF) },
+	{ "LIGHT_CORAL", Color::hex(0xF08080FF) },
+	{ "LIGHT_CYAN", Color::hex(0xE0FFFFFF) },
+	{ "LIGHT_GOLDENROD", Color::hex(0xFAFAD2FF) },
+	{ "LIGHT_GRAY", Color::hex(0xD3D3D3FF) },
+	{ "LIGHT_GREEN", Color::hex(0x90EE90FF) },
+	{ "LIGHT_PINK", Color::hex(0xFFB6C1FF) },
+	{ "LIGHT_SALMON", Color::hex(0xFFA07AFF) },
+	{ "LIGHT_SEA_GREEN", Color::hex(0x20B2AAFF) },
+	{ "LIGHT_SKY_BLUE", Color::hex(0x87CEFAFF) },
+	{ "LIGHT_SLATE_GRAY", Color::hex(0x778899FF) },
+	{ "LIGHT_STEEL_BLUE", Color::hex(0xB0C4DEFF) },
+	{ "LIGHT_YELLOW", Color::hex(0xFFFFE0FF) },
+	{ "LIME", Color::hex(0x00FF00FF) },
+	{ "LIME_GREEN", Color::hex(0x32CD32FF) },
+	{ "LINEN", Color::hex(0xFAF0E6FF) },
+	{ "MAGENTA", Color::hex(0xFF00FFFF) },
+	{ "MAROON", Color::hex(0xB03060FF) },
+	{ "MEDIUM_AQUAMARINE", Color::hex(0x66CDAAFF) },
+	{ "MEDIUM_BLUE", Color::hex(0x0000CDFF) },
+	{ "MEDIUM_ORCHID", Color::hex(0xBA55D3FF) },
+	{ "MEDIUM_PURPLE", Color::hex(0x9370DBFF) },
+	{ "MEDIUM_SEA_GREEN", Color::hex(0x3CB371FF) },
+	{ "MEDIUM_SLATE_BLUE", Color::hex(0x7B68EEFF) },
+	{ "MEDIUM_SPRING_GREEN", Color::hex(0x00FA9AFF) },
+	{ "MEDIUM_TURQUOISE", Color::hex(0x48D1CCFF) },
+	{ "MEDIUM_VIOLET_RED", Color::hex(0xC71585FF) },
+	{ "MIDNIGHT_BLUE", Color::hex(0x191970FF) },
+	{ "MINT_CREAM", Color::hex(0xF5FFFAFF) },
+	{ "MISTY_ROSE", Color::hex(0xFFE4E1FF) },
+	{ "MOCCASIN", Color::hex(0xFFE4B5FF) },
+	{ "NAVAJO_WHITE", Color::hex(0xFFDEADFF) },
+	{ "NAVY_BLUE", Color::hex(0x000080FF) },
+	{ "OLD_LACE", Color::hex(0xFDF5E6FF) },
+	{ "OLIVE", Color::hex(0x808000FF) },
+	{ "OLIVE_DRAB", Color::hex(0x6B8E23FF) },
+	{ "ORANGE", Color::hex(0xFFA500FF) },
+	{ "ORANGE_RED", Color::hex(0xFF4500FF) },
+	{ "ORCHID", Color::hex(0xDA70D6FF) },
+	{ "PALE_GOLDENROD", Color::hex(0xEEE8AAFF) },
+	{ "PALE_GREEN", Color::hex(0x98FB98FF) },
+	{ "PALE_TURQUOISE", Color::hex(0xAFEEEEFF) },
+	{ "PALE_VIOLET_RED", Color::hex(0xDB7093FF) },
+	{ "PAPAYA_WHIP", Color::hex(0xFFEFD5FF) },
+	{ "PEACH_PUFF", Color::hex(0xFFDAB9FF) },
+	{ "PERU", Color::hex(0xCD853FFF) },
+	{ "PINK", Color::hex(0xFFC0CBFF) },
+	{ "PLUM", Color::hex(0xDDA0DDFF) },
+	{ "POWDER_BLUE", Color::hex(0xB0E0E6FF) },
+	{ "PURPLE", Color::hex(0xA020F0FF) },
+	{ "REBECCA_PURPLE", Color::hex(0x663399FF) },
+	{ "RED", Color::hex(0xFF0000FF) },
+	{ "ROSY_BROWN", Color::hex(0xBC8F8FFF) },
+	{ "ROYAL_BLUE", Color::hex(0x4169E1FF) },
+	{ "SADDLE_BROWN", Color::hex(0x8B4513FF) },
+	{ "SALMON", Color::hex(0xFA8072FF) },
+	{ "SANDY_BROWN", Color::hex(0xF4A460FF) },
+	{ "SEA_GREEN", Color::hex(0x2E8B57FF) },
+	{ "SEASHELL", Color::hex(0xFFF5EEFF) },
+	{ "SIENNA", Color::hex(0xA0522DFF) },
+	{ "SILVER", Color::hex(0xC0C0C0FF) },
+	{ "SKY_BLUE", Color::hex(0x87CEEBFF) },
+	{ "SLATE_BLUE", Color::hex(0x6A5ACDFF) },
+	{ "SLATE_GRAY", Color::hex(0x708090FF) },
+	{ "SNOW", Color::hex(0xFFFAFAFF) },
+	{ "SPRING_GREEN", Color::hex(0x00FF7FFF) },
+	{ "STEEL_BLUE", Color::hex(0x4682B4FF) },
+	{ "TAN", Color::hex(0xD2B48CFF) },
+	{ "TEAL", Color::hex(0x008080FF) },
+	{ "THISTLE", Color::hex(0xD8BFD8FF) },
+	{ "TOMATO", Color::hex(0xFF6347FF) },
+	{ "TRANSPARENT", Color::hex(0xFFFFFF00) },
+	{ "TURQUOISE", Color::hex(0x40E0D0FF) },
+	{ "VIOLET", Color::hex(0xEE82EEFF) },
+	{ "WEB_GRAY", Color::hex(0x808080FF) },
+	{ "WEB_GREEN", Color::hex(0x008000FF) },
+	{ "WEB_MAROON", Color::hex(0x800000FF) },
+	{ "WEB_PURPLE", Color::hex(0x800080FF) },
+	{ "WHEAT", Color::hex(0xF5DEB3FF) },
+	{ "WHITE", Color::hex(0xFFFFFFFF) },
+	{ "WHITE_SMOKE", Color::hex(0xF5F5F5FF) },
+	{ "YELLOW", Color::hex(0xFFFF00FF) },
+	{ "YELLOW_GREEN", Color::hex(0x9ACD32FF) },
+};

core/math/convex_hull.h

@@ -0,0 +1,113 @@
+/**************************************************************************/
+/*  convex_hull.h                                                         */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#pragma once
+
+/*
+Copyright (c) 2011 Ole Kniemeyer, MAXON, www.maxon.net
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "core/math/geometry_3d.h"
+#include "core/math/vector3.h"
+#include "core/templates/local_vector.h"
+#include "core/templates/vector.h"
+
+/// Convex hull implementation based on Preparata and Hong
+/// See https://code.google.com/archive/p/bullet/issues/275
+/// Ole Kniemeyer, MAXON Computer GmbH
+class ConvexHullComputer {
+public:
+	class Edge {
+	private:
+		int32_t next = 0;
+		int32_t reverse = 0;
+		int32_t target_vertex = 0;
+
+		friend class ConvexHullComputer;
+
+	public:
+		int32_t get_next_relative() const {
+			return next;
+		}
+
+		int32_t get_source_vertex() const {
+			return (this + reverse)->target_vertex;
+		}
+
+		int32_t get_target_vertex() const {
+			return target_vertex;
+		}
+
+		const Edge *get_next_edge_of_vertex() const // clockwise list of all edges of a vertex
+		{
+			return this + next;
+		}
+
+		const Edge *get_next_edge_of_face() const // counter-clockwise list of all edges of a face
+		{
+			return (this + reverse)->get_next_edge_of_vertex();
+		}
+
+		const Edge *get_reverse_edge() const {
+			return this + reverse;
+		}
+	};
+
+	// Vertices of the output hull
+	LocalVector<Vector3> vertices;
+
+	// Edges of the output hull
+	LocalVector<Edge> edges;
+
+	// Faces of the convex hull. Each entry is an index into the "edges" array pointing to an edge of the face. Faces are planar n-gons
+	LocalVector<int32_t> faces;
+
+	/*
+		Compute convex hull of "count" vertices stored in "coords".
+		If "shrink" is positive, the convex hull is shrunken by that amount (each face is moved by "shrink" length units
+		towards the center along its normal).
+		If "shrinkClamp" is positive, "shrink" is clamped to not exceed "shrinkClamp * innerRadius", where "innerRadius"
+		is the minimum distance of a face to the center of the convex hull.
+		The returned value is the amount by which the hull has been shrunken. If it is negative, the amount was so large
+		that the resulting convex hull is empty.
+		The output convex hull can be found in the member variables "vertices", "edges", "faces".
+		*/
+	real_t compute(const Vector3 *p_coords, int32_t p_count, real_t p_shrink, real_t p_shrink_clamp);
+
+	static Error convex_hull(const Vector<Vector3> &p_points, Geometry3D::MeshData &r_mesh);
+};

core/math/delaunay_2d.h

@@ -0,0 +1,157 @@
+/**************************************************************************/
+/*  delaunay_2d.h                                                         */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "core/math/rect2.h"
+#include "core/templates/vector.h"
+
+class Delaunay2D {
+public:
+	struct Triangle {
+		int points[3];
+		Vector2 circum_center;
+		real_t circum_radius_squared;
+		Triangle() {}
+		Triangle(int p_a, int p_b, int p_c) {
+			points[0] = p_a;
+			points[1] = p_b;
+			points[2] = p_c;
+		}
+	};
+
+	struct Edge {
+		int points[2];
+		bool bad = false;
+		Edge() {}
+		Edge(int p_a, int p_b) {
+			// Store indices in a sorted manner to avoid having to check both orientations later.
+			if (p_a > p_b) {
+				points[0] = p_b;
+				points[1] = p_a;
+			} else {
+				points[0] = p_a;
+				points[1] = p_b;
+			}
+		}
+	};
+
+	static Triangle create_triangle(const Vector<Vector2> &p_vertices, int p_a, int p_b, int p_c) {
+		Triangle triangle = Triangle(p_a, p_b, p_c);
+
+		// Get the values of the circumcircle and store them inside the triangle object.
+		Vector2 a = p_vertices[p_b] - p_vertices[p_a];
+		Vector2 b = p_vertices[p_c] - p_vertices[p_a];
+
+		Vector2 O = (b * a.length_squared() - a * b.length_squared()).orthogonal() / (a.cross(b) * 2.0f);
+
+		triangle.circum_radius_squared = O.length_squared();
+		triangle.circum_center = O + p_vertices[p_a];
+
+		return triangle;
+	}
+
+	static Vector<Triangle> triangulate(const Vector<Vector2> &p_points) {
+		Vector<Vector2> points = p_points;
+		Vector<Triangle> triangles;
+
+		int point_count = p_points.size();
+		if (point_count <= 2) {
+			return triangles;
+		}
+
+		// Get a bounding rectangle.
+		Rect2 rect = Rect2(p_points[0], Size2());
+		for (int i = 1; i < point_count; i++) {
+			rect.expand_to(p_points[i]);
+		}
+
+		real_t delta_max = MAX(rect.size.width, rect.size.height);
+		Vector2 center = rect.get_center();
+
+		// Construct a bounding triangle around the rectangle.
+		points.push_back(Vector2(center.x - delta_max * 16, center.y - delta_max));
+		points.push_back(Vector2(center.x, center.y + delta_max * 16));
+		points.push_back(Vector2(center.x + delta_max * 16, center.y - delta_max));
+
+		Triangle bounding_triangle = create_triangle(points, point_count + 0, point_count + 1, point_count + 2);
+		triangles.push_back(bounding_triangle);
+
+		for (int i = 0; i < point_count; i++) {
+			Vector<Edge> polygon;
+
+			// Save the edges of the triangles whose circumcircles contain the i-th vertex. Delete the triangles themselves.
+			for (int j = triangles.size() - 1; j >= 0; j--) {
+				if (points[i].distance_squared_to(triangles[j].circum_center) < triangles[j].circum_radius_squared) {
+					polygon.push_back(Edge(triangles[j].points[0], triangles[j].points[1]));
+					polygon.push_back(Edge(triangles[j].points[1], triangles[j].points[2]));
+					polygon.push_back(Edge(triangles[j].points[2], triangles[j].points[0]));
+
+					triangles.remove_at(j);
+				}
+			}
+
+			// Create a triangle for every unique edge.
+			for (int j = 0; j < polygon.size(); j++) {
+				if (polygon[j].bad) {
+					continue;
+				}
+
+				for (int k = j + 1; k < polygon.size(); k++) {
+					// Compare the edges.
+					if (polygon[k].points[0] == polygon[j].points[0] && polygon[k].points[1] == polygon[j].points[1]) {
+						polygon.write[j].bad = true;
+						polygon.write[k].bad = true;
+
+						break; // Since no more than two triangles can share an edge, no more than two edges can share vertices.
+					}
+				}
+
+				// Create triangles out of good edges.
+				if (!polygon[j].bad) {
+					triangles.push_back(create_triangle(points, polygon[j].points[0], polygon[j].points[1], i));
+				}
+			}
+		}
+
+		// Filter out the triangles containing vertices of the bounding triangle.
+		int preserved_count = 0;
+		Triangle *triangles_ptrw = triangles.ptrw();
+		for (int i = 0; i < triangles.size(); i++) {
+			if (!(triangles[i].points[0] >= point_count || triangles[i].points[1] >= point_count || triangles[i].points[2] >= point_count)) {
+				triangles_ptrw[preserved_count] = triangles[i];
+				preserved_count++;
+			}
+		}
+		triangles.resize(preserved_count);
+
+		return triangles;
+	}
+};

core/math/delaunay_3d.h

@@ -0,0 +1,389 @@
+/**************************************************************************/
+/*  delaunay_3d.h                                                         */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "core/math/aabb.h"
+#include "core/math/projection.h"
+#include "core/math/vector3.h"
+#include "core/templates/a_hash_map.h"
+#include "core/templates/list.h"
+#include "core/templates/local_vector.h"
+#include "core/templates/vector.h"
+
+#include "thirdparty/misc/r128.h"
+
+class Delaunay3D {
+	struct Simplex;
+
+	enum {
+		ACCEL_GRID_SIZE = 16,
+		QUANTIZATION_MAX = 1 << 16 // A power of two smaller than the 23 bit significand of a float.
+	};
+	struct GridPos {
+		Vector3i pos;
+		List<Simplex *>::Element *E = nullptr;
+	};
+
+	struct Simplex {
+		uint32_t points[4];
+		R128 circum_center_x;
+		R128 circum_center_y;
+		R128 circum_center_z;
+		R128 circum_r2;
+		LocalVector<GridPos> grid_positions;
+		List<Simplex *>::Element *SE = nullptr;
+
+		_FORCE_INLINE_ Simplex() {}
+		_FORCE_INLINE_ Simplex(uint32_t p_a, uint32_t p_b, uint32_t p_c, uint32_t p_d) {
+			points[0] = p_a;
+			points[1] = p_b;
+			points[2] = p_c;
+			points[3] = p_d;
+		}
+	};
+
+	struct Triangle {
+		uint32_t triangle[3];
+		bool bad = false;
+		_FORCE_INLINE_ bool operator==(const Triangle &p_triangle) const {
+			return triangle[0] == p_triangle.triangle[0] && triangle[1] == p_triangle.triangle[1] && triangle[2] == p_triangle.triangle[2];
+		}
+
+		_FORCE_INLINE_ Triangle() {}
+		_FORCE_INLINE_ Triangle(uint32_t p_a, uint32_t p_b, uint32_t p_c) {
+			if (p_a > p_b) {
+				SWAP(p_a, p_b);
+			}
+			if (p_b > p_c) {
+				SWAP(p_b, p_c);
+			}
+			if (p_a > p_b) {
+				SWAP(p_a, p_b);
+			}
+
+			triangle[0] = p_a;
+			triangle[1] = p_b;
+			triangle[2] = p_c;
+		}
+	};
+
+	struct TriangleHasher {
+		_FORCE_INLINE_ static uint32_t hash(const Triangle &p_triangle) {
+			uint32_t h = hash_djb2_one_32(p_triangle.triangle[0]);
+			h = hash_djb2_one_32(p_triangle.triangle[1], h);
+			return hash_fmix32(hash_djb2_one_32(p_triangle.triangle[2], h));
+		}
+	};
+
+	_FORCE_INLINE_ static void circum_sphere_compute(const Vector3 *p_points, Simplex *p_simplex) {
+		// The only part in the algorithm where there may be precision errors is this one,
+		// so ensure that we do it with the maximum precision possible.
+
+		R128 v0_x = p_points[p_simplex->points[0]].x;
+		R128 v0_y = p_points[p_simplex->points[0]].y;
+		R128 v0_z = p_points[p_simplex->points[0]].z;
+		R128 v1_x = p_points[p_simplex->points[1]].x;
+		R128 v1_y = p_points[p_simplex->points[1]].y;
+		R128 v1_z = p_points[p_simplex->points[1]].z;
+		R128 v2_x = p_points[p_simplex->points[2]].x;
+		R128 v2_y = p_points[p_simplex->points[2]].y;
+		R128 v2_z = p_points[p_simplex->points[2]].z;
+		R128 v3_x = p_points[p_simplex->points[3]].x;
+		R128 v3_y = p_points[p_simplex->points[3]].y;
+		R128 v3_z = p_points[p_simplex->points[3]].z;
+
+		// Create the rows of our "unrolled" 3x3 matrix.
+		R128 row1_x = v1_x - v0_x;
+		R128 row1_y = v1_y - v0_y;
+		R128 row1_z = v1_z - v0_z;
+
+		R128 row2_x = v2_x - v0_x;
+		R128 row2_y = v2_y - v0_y;
+		R128 row2_z = v2_z - v0_z;
+
+		R128 row3_x = v3_x - v0_x;
+		R128 row3_y = v3_y - v0_y;
+		R128 row3_z = v3_z - v0_z;
+
+		R128 sq_length1 = row1_x * row1_x + row1_y * row1_y + row1_z * row1_z;
+		R128 sq_length2 = row2_x * row2_x + row2_y * row2_y + row2_z * row2_z;
+		R128 sq_length3 = row3_x * row3_x + row3_y * row3_y + row3_z * row3_z;
+
+		// Compute the determinant of said matrix.
+		R128 determinant = row1_x * (row2_y * row3_z - row3_y * row2_z) - row2_x * (row1_y * row3_z - row3_y * row1_z) + row3_x * (row1_y * row2_z - row2_y * row1_z);
+
+		// Compute the volume of the tetrahedron, and precompute a scalar quantity for reuse in the formula.
+		R128 volume = determinant / R128(6.f);
+		R128 i12volume = R128(1.f) / (volume * R128(12.f));
+
+		R128 center_x = v0_x + i12volume * ((row2_y * row3_z - row3_y * row2_z) * sq_length1 - (row1_y * row3_z - row3_y * row1_z) * sq_length2 + (row1_y * row2_z - row2_y * row1_z) * sq_length3);
+		R128 center_y = v0_y + i12volume * (-(row2_x * row3_z - row3_x * row2_z) * sq_length1 + (row1_x * row3_z - row3_x * row1_z) * sq_length2 - (row1_x * row2_z - row2_x * row1_z) * sq_length3);
+		R128 center_z = v0_z + i12volume * ((row2_x * row3_y - row3_x * row2_y) * sq_length1 - (row1_x * row3_y - row3_x * row1_y) * sq_length2 + (row1_x * row2_y - row2_x * row1_y) * sq_length3);
+
+		// Once we know the center, the radius is clearly the distance to any vertex.
+		R128 rel1_x = center_x - v0_x;
+		R128 rel1_y = center_y - v0_y;
+		R128 rel1_z = center_z - v0_z;
+
+		R128 radius1 = rel1_x * rel1_x + rel1_y * rel1_y + rel1_z * rel1_z;
+
+		p_simplex->circum_center_x = center_x;
+		p_simplex->circum_center_y = center_y;
+		p_simplex->circum_center_z = center_z;
+		p_simplex->circum_r2 = radius1;
+	}
+
+	_FORCE_INLINE_ static bool simplex_contains(const Vector3 *p_points, const Simplex &p_simplex, uint32_t p_vertex) {
+		R128 v_x = p_points[p_vertex].x;
+		R128 v_y = p_points[p_vertex].y;
+		R128 v_z = p_points[p_vertex].z;
+
+		R128 rel2_x = p_simplex.circum_center_x - v_x;
+		R128 rel2_y = p_simplex.circum_center_y - v_y;
+		R128 rel2_z = p_simplex.circum_center_z - v_z;
+
+		R128 radius2 = rel2_x * rel2_x + rel2_y * rel2_y + rel2_z * rel2_z;
+
+		return radius2 < (p_simplex.circum_r2 - R128(0.0000000001));
+		// When this tolerance is too big, it can result in overlapping simplices.
+		// When it's too small, large amounts of planar simplices are created.
+	}
+
+	static bool simplex_is_coplanar(const Vector3 *p_points, const Simplex &p_simplex) {
+		// Checking every possible distance like this is overkill, but only checking
+		// one is not enough. If the simplex is almost planar then the vectors p1-p2
+		// and p1-p3 can be practically collinear, which makes Plane unreliable.
+		for (uint32_t i = 0; i < 4; i++) {
+			Plane p(p_points[p_simplex.points[i]], p_points[p_simplex.points[(i + 1) % 4]], p_points[p_simplex.points[(i + 2) % 4]]);
+			// This tolerance should not be smaller than the one used with
+			// Plane::has_point() when creating the LightmapGI probe BSP tree.
+			if (Math::abs(p.distance_to(p_points[p_simplex.points[(i + 3) % 4]])) < 0.001) {
+				return true;
+			}
+		}
+
+		return false;
+	}
+
+public:
+	struct OutputSimplex {
+		uint32_t points[4];
+	};
+
+	static Vector<OutputSimplex> tetrahedralize(const Vector<Vector3> &p_points) {
+		uint32_t point_count = p_points.size();
+		Vector3 *points = (Vector3 *)memalloc(sizeof(Vector3) * (point_count + 4));
+		const Vector3 *src_points = p_points.ptr();
+		Vector3 proportions;
+
+		{
+			AABB rect;
+			for (uint32_t i = 0; i < point_count; i++) {
+				Vector3 point = src_points[i];
+				if (i == 0) {
+					rect.position = point;
+				} else {
+					rect.expand_to(point);
+				}
+			}
+
+			real_t longest_axis = rect.size[rect.get_longest_axis_index()];
+			proportions = Vector3(longest_axis, longest_axis, longest_axis) / rect.size;
+
+			for (uint32_t i = 0; i < point_count; i++) {
+				// Scale points to the unit cube to better utilize R128 precision
+				// and quantize to stabilize triangulation over a wide range of
+				// distances.
+				points[i] = Vector3(Vector3i((src_points[i] - rect.position) / longest_axis * QUANTIZATION_MAX)) / QUANTIZATION_MAX;
+			}
+
+			const real_t delta_max = Math::sqrt(2.0) * 100.0;
+			Vector3 center = Vector3(0.5, 0.5, 0.5);
+
+			// The larger the root simplex is, the more likely it is that the
+			// triangulation is convex. If it's not absolutely huge, there can
+			// be missing simplices that are not created for the outermost faces
+			// of the point cloud if the point density is very low there.
+			points[point_count + 0] = center + Vector3(0, 1, 0) * delta_max;
+			points[point_count + 1] = center + Vector3(0, -1, 1) * delta_max;
+			points[point_count + 2] = center + Vector3(1, -1, -1) * delta_max;
+			points[point_count + 3] = center + Vector3(-1, -1, -1) * delta_max;
+		}
+
+		List<Simplex *> acceleration_grid[ACCEL_GRID_SIZE][ACCEL_GRID_SIZE][ACCEL_GRID_SIZE];
+
+		List<Simplex *> simplex_list;
+		{
+			//create root simplex
+			Simplex *root = memnew(Simplex(point_count + 0, point_count + 1, point_count + 2, point_count + 3));
+			root->SE = simplex_list.push_back(root);
+
+			for (uint32_t i = 0; i < ACCEL_GRID_SIZE; i++) {
+				for (uint32_t j = 0; j < ACCEL_GRID_SIZE; j++) {
+					for (uint32_t k = 0; k < ACCEL_GRID_SIZE; k++) {
+						GridPos gp;
+						gp.E = acceleration_grid[i][j][k].push_back(root);
+						gp.pos = Vector3i(i, j, k);
+						root->grid_positions.push_back(gp);
+					}
+				}
+			}
+
+			circum_sphere_compute(points, root);
+		}
+
+		AHashMap<Triangle, uint32_t, TriangleHasher> triangles_inserted;
+		LocalVector<Triangle> triangles;
+
+		for (uint32_t i = 0; i < point_count; i++) {
+			bool unique = true;
+			for (uint32_t j = i + 1; j < point_count; j++) {
+				if (points[i] == points[j]) {
+					unique = false;
+					break;
+				}
+			}
+			if (!unique) {
+				continue;
+			}
+
+			Vector3i grid_pos = Vector3i(points[i] * proportions * ACCEL_GRID_SIZE);
+			grid_pos = grid_pos.clampi(0, ACCEL_GRID_SIZE - 1);
+
+			for (List<Simplex *>::Element *E = acceleration_grid[grid_pos.x][grid_pos.y][grid_pos.z].front(); E;) {
+				List<Simplex *>::Element *N = E->next(); //may be deleted
+
+				Simplex *simplex = E->get();
+
+				if (simplex_contains(points, *simplex, i)) {
+					static const uint32_t triangle_order[4][3] = {
+						{ 0, 1, 2 },
+						{ 0, 1, 3 },
+						{ 0, 2, 3 },
+						{ 1, 2, 3 },
+					};
+
+					for (uint32_t k = 0; k < 4; k++) {
+						Triangle t = Triangle(simplex->points[triangle_order[k][0]], simplex->points[triangle_order[k][1]], simplex->points[triangle_order[k][2]]);
+						uint32_t *p = triangles_inserted.getptr(t);
+						if (p) {
+							// This Delaunay implementation uses the Bowyer-Watson algorithm.
+							// The rule is that you don't reuse any triangles that were
+							// shared by any of the retriangulated simplices.
+							triangles[*p].bad = true;
+						} else {
+							triangles_inserted.insert(t, triangles.size());
+							triangles.push_back(t);
+						}
+					}
+
+					simplex_list.erase(simplex->SE);
+
+					for (const GridPos &gp : simplex->grid_positions) {
+						Vector3i p = gp.pos;
+						acceleration_grid[p.x][p.y][p.z].erase(gp.E);
+					}
+					memdelete(simplex);
+				}
+				E = N;
+			}
+
+			for (const Triangle &triangle : triangles) {
+				if (triangle.bad) {
+					continue;
+				}
+				Simplex *new_simplex = memnew(Simplex(triangle.triangle[0], triangle.triangle[1], triangle.triangle[2], i));
+				circum_sphere_compute(points, new_simplex);
+				new_simplex->SE = simplex_list.push_back(new_simplex);
+				{
+					Vector3 center;
+					center.x = double(new_simplex->circum_center_x);
+					center.y = double(new_simplex->circum_center_y);
+					center.z = double(new_simplex->circum_center_z);
+
+					const real_t radius2 = Math::sqrt(double(new_simplex->circum_r2)) + 0.0001;
+					Vector3 extents = Vector3(radius2, radius2, radius2);
+					Vector3i from = Vector3i((center - extents) * proportions * ACCEL_GRID_SIZE);
+					Vector3i to = Vector3i((center + extents) * proportions * ACCEL_GRID_SIZE);
+					from = from.clampi(0, ACCEL_GRID_SIZE - 1);
+					to = to.clampi(0, ACCEL_GRID_SIZE - 1);
+
+					for (int32_t x = from.x; x <= to.x; x++) {
+						for (int32_t y = from.y; y <= to.y; y++) {
+							for (int32_t z = from.z; z <= to.z; z++) {
+								GridPos gp;
+								gp.pos = Vector3(x, y, z);
+								gp.E = acceleration_grid[x][y][z].push_back(new_simplex);
+								new_simplex->grid_positions.push_back(gp);
+							}
+						}
+					}
+				}
+			}
+
+			triangles.clear();
+			triangles_inserted.clear();
+		}
+
+		//print_line("end with simplices: " + itos(simplex_list.size()));
+		Vector<OutputSimplex> ret_simplices;
+		ret_simplices.resize(simplex_list.size());
+		OutputSimplex *ret_simplicesw = ret_simplices.ptrw();
+		uint32_t simplices_written = 0;
+
+		for (Simplex *simplex : simplex_list) {
+			bool invalid = false;
+			for (int j = 0; j < 4; j++) {
+				if (simplex->points[j] >= point_count) {
+					invalid = true;
+					break;
+				}
+			}
+			if (invalid || simplex_is_coplanar(src_points, *simplex)) {
+				memdelete(simplex);
+				continue;
+			}
+
+			ret_simplicesw[simplices_written].points[0] = simplex->points[0];
+			ret_simplicesw[simplices_written].points[1] = simplex->points[1];
+			ret_simplicesw[simplices_written].points[2] = simplex->points[2];
+			ret_simplicesw[simplices_written].points[3] = simplex->points[3];
+			simplices_written++;
+			memdelete(simplex);
+		}
+
+		ret_simplices.resize(simplices_written);
+
+		memfree(points);
+
+		return ret_simplices;
+	}
+};

core/math/disjoint_set.h

@@ -0,0 +1,147 @@
+/**************************************************************************/
+/*  disjoint_set.h                                                        */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "core/templates/hash_map.h"
+#include "core/templates/vector.h"
+
+/* This DisjointSet class uses Find with path compression and Union by rank */
+template <typename T, typename H = HashMapHasherDefault, typename C = HashMapComparatorDefault<T>, typename AL = DefaultAllocator>
+class DisjointSet {
+	struct Element {
+		T object;
+		Element *parent = nullptr;
+		int rank = 0;
+	};
+
+	typedef HashMap<T, Element *, H, C> MapT;
+
+	MapT elements;
+
+	Element *get_parent(Element *element);
+
+	_FORCE_INLINE_ Element *insert_or_get(T object);
+
+public:
+	~DisjointSet();
+
+	_FORCE_INLINE_ void insert(T object) { (void)insert_or_get(object); }
+
+	void create_union(T a, T b);
+
+	void get_representatives(Vector<T> &out_roots);
+
+	void get_members(Vector<T> &out_members, T representative);
+};
+
+/* FUNCTIONS */
+
+template <typename T, typename H, typename C, typename AL>
+DisjointSet<T, H, C, AL>::~DisjointSet() {
+	for (KeyValue<T, Element *> &E : elements) {
+		memdelete_allocator<Element, AL>(E.value);
+	}
+}
+
+template <typename T, typename H, typename C, typename AL>
+typename DisjointSet<T, H, C, AL>::Element *DisjointSet<T, H, C, AL>::get_parent(Element *element) {
+	if (element->parent != element) {
+		element->parent = get_parent(element->parent);
+	}
+
+	return element->parent;
+}
+
+template <typename T, typename H, typename C, typename AL>
+typename DisjointSet<T, H, C, AL>::Element *DisjointSet<T, H, C, AL>::insert_or_get(T object) {
+	typename MapT::Iterator itr = elements.find(object);
+	if (itr != nullptr) {
+		return itr->value;
+	}
+
+	Element *new_element = memnew_allocator(Element, AL);
+	new_element->object = object;
+	new_element->parent = new_element;
+	elements.insert(object, new_element);
+
+	return new_element;
+}
+
+template <typename T, typename H, typename C, typename AL>
+void DisjointSet<T, H, C, AL>::create_union(T a, T b) {
+	Element *x = insert_or_get(a);
+	Element *y = insert_or_get(b);
+
+	Element *x_root = get_parent(x);
+	Element *y_root = get_parent(y);
+
+	// Already in the same set
+	if (x_root == y_root) {
+		return;
+	}
+
+	// Not in the same set, merge
+	if (x_root->rank < y_root->rank) {
+		SWAP(x_root, y_root);
+	}
+
+	// Merge y_root into x_root
+	y_root->parent = x_root;
+	if (x_root->rank == y_root->rank) {
+		++x_root->rank;
+	}
+}
+
+template <typename T, typename H, typename C, typename AL>
+void DisjointSet<T, H, C, AL>::get_representatives(Vector<T> &out_representatives) {
+	for (KeyValue<T, Element *> &E : elements) {
+		Element *element = E.value;
+		if (element->parent == element) {
+			out_representatives.push_back(element->object);
+		}
+	}
+}
+
+template <typename T, typename H, typename C, typename AL>
+void DisjointSet<T, H, C, AL>::get_members(Vector<T> &out_members, T representative) {
+	typename MapT::Iterator rep_itr = elements.find(representative);
+	ERR_FAIL_NULL(rep_itr);
+
+	Element *rep_element = rep_itr->value;
+	ERR_FAIL_COND(rep_element->parent != rep_element);
+
+	for (KeyValue<T, Element *> &E : elements) {
+		Element *parent = get_parent(E.value);
+		if (parent == rep_element) {
+			out_members.push_back(E.key);
+		}
+	}
+}

core/math/dynamic_bvh.cpp

@@ -0,0 +1,439 @@
+/**************************************************************************/
+/*  dynamic_bvh.cpp                                                       */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "dynamic_bvh.h"
+
+void DynamicBVH::_delete_node(Node *p_node) {
+	node_allocator.free(p_node);
+}
+
+void DynamicBVH::_recurse_delete_node(Node *p_node) {
+	if (!p_node->is_leaf()) {
+		_recurse_delete_node(p_node->children[0]);
+		_recurse_delete_node(p_node->children[1]);
+	}
+	if (p_node == bvh_root) {
+		bvh_root = nullptr;
+	}
+	_delete_node(p_node);
+}
+
+DynamicBVH::Node *DynamicBVH::_create_node(Node *p_parent, void *p_data) {
+	Node *node = node_allocator.alloc();
+	node->parent = p_parent;
+	node->data = p_data;
+	return (node);
+}
+
+DynamicBVH::Node *DynamicBVH::_create_node_with_volume(Node *p_parent, const Volume &p_volume, void *p_data) {
+	Node *node = _create_node(p_parent, p_data);
+	node->volume = p_volume;
+	return node;
+}
+
+void DynamicBVH::_insert_leaf(Node *p_root, Node *p_leaf) {
+	if (!bvh_root) {
+		bvh_root = p_leaf;
+		p_leaf->parent = nullptr;
+	} else {
+		if (!p_root->is_leaf()) {
+			do {
+				p_root = p_root->children[p_leaf->volume.select_by_proximity(
+						p_root->children[0]->volume,
+						p_root->children[1]->volume)];
+			} while (!p_root->is_leaf());
+		}
+		Node *prev = p_root->parent;
+		Node *node = _create_node_with_volume(prev, p_leaf->volume.merge(p_root->volume), nullptr);
+		if (prev) {
+			prev->children[p_root->get_index_in_parent()] = node;
+			node->children[0] = p_root;
+			p_root->parent = node;
+			node->children[1] = p_leaf;
+			p_leaf->parent = node;
+			do {
+				if (!prev->volume.contains(node->volume)) {
+					prev->volume = prev->children[0]->volume.merge(prev->children[1]->volume);
+				} else {
+					break;
+				}
+				node = prev;
+			} while (nullptr != (prev = node->parent));
+		} else {
+			node->children[0] = p_root;
+			p_root->parent = node;
+			node->children[1] = p_leaf;
+			p_leaf->parent = node;
+			bvh_root = node;
+		}
+	}
+}
+
+DynamicBVH::Node *DynamicBVH::_remove_leaf(Node *leaf) {
+	if (leaf == bvh_root) {
+		bvh_root = nullptr;
+		return (nullptr);
+	} else {
+		Node *parent = leaf->parent;
+		Node *prev = parent->parent;
+		Node *sibling = parent->children[1 - leaf->get_index_in_parent()];
+		if (prev) {
+			prev->children[parent->get_index_in_parent()] = sibling;
+			sibling->parent = prev;
+			_delete_node(parent);
+			while (prev) {
+				const Volume pb = prev->volume;
+				prev->volume = prev->children[0]->volume.merge(prev->children[1]->volume);
+				if (pb.is_not_equal_to(prev->volume)) {
+					prev = prev->parent;
+				} else {
+					break;
+				}
+			}
+			return (prev ? prev : bvh_root);
+		} else {
+			bvh_root = sibling;
+			sibling->parent = nullptr;
+			_delete_node(parent);
+			return (bvh_root);
+		}
+	}
+}
+
+void DynamicBVH::_fetch_leaves(Node *p_root, LocalVector<Node *> &r_leaves, int p_depth) {
+	if (p_root->is_internal() && p_depth) {
+		_fetch_leaves(p_root->children[0], r_leaves, p_depth - 1);
+		_fetch_leaves(p_root->children[1], r_leaves, p_depth - 1);
+		_delete_node(p_root);
+	} else {
+		r_leaves.push_back(p_root);
+	}
+}
+
+// Partitions leaves such that leaves[0, n) are on the
+// left of axis, and leaves[n, count) are on the right
+// of axis. returns N.
+int DynamicBVH::_split(Node **leaves, int p_count, const Vector3 &p_org, const Vector3 &p_axis) {
+	int begin = 0;
+	int end = p_count;
+	for (;;) {
+		while (begin != end && leaves[begin]->is_left_of_axis(p_org, p_axis)) {
+			++begin;
+		}
+
+		if (begin == end) {
+			break;
+		}
+
+		while (begin != end && !leaves[end - 1]->is_left_of_axis(p_org, p_axis)) {
+			--end;
+		}
+
+		if (begin == end) {
+			break;
+		}
+
+		// swap out of place nodes
+		--end;
+		Node *temp = leaves[begin];
+		leaves[begin] = leaves[end];
+		leaves[end] = temp;
+		++begin;
+	}
+
+	return begin;
+}
+
+DynamicBVH::Volume DynamicBVH::_bounds(Node **leaves, int p_count) {
+	Volume volume = leaves[0]->volume;
+	for (int i = 1, ni = p_count; i < ni; ++i) {
+		volume = volume.merge(leaves[i]->volume);
+	}
+	return (volume);
+}
+
+void DynamicBVH::_bottom_up(Node **leaves, int p_count) {
+	while (p_count > 1) {
+		real_t minsize = Math::INF;
+		int minidx[2] = { -1, -1 };
+		for (int i = 0; i < p_count; ++i) {
+			for (int j = i + 1; j < p_count; ++j) {
+				const real_t sz = leaves[i]->volume.merge(leaves[j]->volume).get_size();
+				if (sz < minsize) {
+					minsize = sz;
+					minidx[0] = i;
+					minidx[1] = j;
+				}
+			}
+		}
+		Node *n[] = { leaves[minidx[0]], leaves[minidx[1]] };
+		Node *p = _create_node_with_volume(nullptr, n[0]->volume.merge(n[1]->volume), nullptr);
+		p->children[0] = n[0];
+		p->children[1] = n[1];
+		n[0]->parent = p;
+		n[1]->parent = p;
+		leaves[minidx[0]] = p;
+		leaves[minidx[1]] = leaves[p_count - 1];
+		--p_count;
+	}
+}
+
+DynamicBVH::Node *DynamicBVH::_top_down(Node **leaves, int p_count, int p_bu_threshold) {
+	static const Vector3 axis[] = { Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1) };
+
+	ERR_FAIL_COND_V(p_bu_threshold <= 1, nullptr);
+	if (p_count > 1) {
+		if (p_count > p_bu_threshold) {
+			const Volume vol = _bounds(leaves, p_count);
+			const Vector3 org = vol.get_center();
+			int partition;
+			int bestaxis = -1;
+			int bestmidp = p_count;
+			int splitcount[3][2] = { { 0, 0 }, { 0, 0 }, { 0, 0 } };
+			int i;
+			for (i = 0; i < p_count; ++i) {
+				const Vector3 x = leaves[i]->volume.get_center() - org;
+				for (int j = 0; j < 3; ++j) {
+					++splitcount[j][x.dot(axis[j]) > 0 ? 1 : 0];
+				}
+			}
+			for (i = 0; i < 3; ++i) {
+				if ((splitcount[i][0] > 0) && (splitcount[i][1] > 0)) {
+					const int midp = (int)Math::abs(real_t(splitcount[i][0] - splitcount[i][1]));
+					if (midp < bestmidp) {
+						bestaxis = i;
+						bestmidp = midp;
+					}
+				}
+			}
+			if (bestaxis >= 0) {
+				partition = _split(leaves, p_count, org, axis[bestaxis]);
+				ERR_FAIL_COND_V(partition == 0 || partition == p_count, nullptr);
+			} else {
+				partition = p_count / 2 + 1;
+			}
+
+			Node *node = _create_node_with_volume(nullptr, vol, nullptr);
+			node->children[0] = _top_down(&leaves[0], partition, p_bu_threshold);
+			node->children[1] = _top_down(&leaves[partition], p_count - partition, p_bu_threshold);
+			node->children[0]->parent = node;
+			node->children[1]->parent = node;
+			return (node);
+		} else {
+			_bottom_up(leaves, p_count);
+			return (leaves[0]);
+		}
+	}
+	return (leaves[0]);
+}
+
+DynamicBVH::Node *DynamicBVH::_node_sort(Node *n, Node *&r) {
+	Node *p = n->parent;
+	ERR_FAIL_COND_V(!n->is_internal(), nullptr);
+	if (p > n) {
+		const int i = n->get_index_in_parent();
+		const int j = 1 - i;
+		Node *s = p->children[j];
+		Node *q = p->parent;
+		ERR_FAIL_COND_V(n != p->children[i], nullptr);
+		if (q) {
+			q->children[p->get_index_in_parent()] = n;
+		} else {
+			r = n;
+		}
+		s->parent = n;
+		p->parent = n;
+		n->parent = q;
+		p->children[0] = n->children[0];
+		p->children[1] = n->children[1];
+		n->children[0]->parent = p;
+		n->children[1]->parent = p;
+		n->children[i] = p;
+		n->children[j] = s;
+		SWAP(p->volume, n->volume);
+		return (p);
+	}
+	return (n);
+}
+
+void DynamicBVH::clear() {
+	if (bvh_root) {
+		_recurse_delete_node(bvh_root);
+	}
+	lkhd = -1;
+	opath = 0;
+}
+
+void DynamicBVH::optimize_bottom_up() {
+	if (bvh_root) {
+		LocalVector<Node *> leaves;
+		_fetch_leaves(bvh_root, leaves);
+		_bottom_up(&leaves[0], leaves.size());
+		bvh_root = leaves[0];
+	}
+}
+
+void DynamicBVH::optimize_top_down(int bu_threshold) {
+	if (bvh_root) {
+		LocalVector<Node *> leaves;
+		_fetch_leaves(bvh_root, leaves);
+		bvh_root = _top_down(&leaves[0], leaves.size(), bu_threshold);
+	}
+}
+
+void DynamicBVH::optimize_incremental(int passes) {
+	if (passes < 0) {
+		passes = total_leaves;
+	}
+	if (passes > 0) {
+		do {
+			if (!bvh_root) {
+				break;
+			}
+			Node *node = bvh_root;
+			unsigned bit = 0;
+			while (node->is_internal()) {
+				node = _node_sort(node, bvh_root)->children[(opath >> bit) & 1];
+				bit = (bit + 1) & (sizeof(unsigned) * 8 - 1);
+			}
+			_update(node);
+			++opath;
+		} while (--passes);
+	}
+}
+
+DynamicBVH::ID DynamicBVH::insert(const AABB &p_box, void *p_userdata) {
+	Volume volume;
+	volume.min = p_box.position;
+	volume.max = p_box.position + p_box.size;
+
+	Node *leaf = _create_node_with_volume(nullptr, volume, p_userdata);
+	_insert_leaf(bvh_root, leaf);
+	++total_leaves;
+
+	ID id;
+	id.node = leaf;
+
+	return id;
+}
+
+void DynamicBVH::_update(Node *leaf, int lookahead) {
+	Node *root = _remove_leaf(leaf);
+	if (root) {
+		if (lookahead >= 0) {
+			for (int i = 0; (i < lookahead) && root->parent; ++i) {
+				root = root->parent;
+			}
+		} else {
+			root = bvh_root;
+		}
+	}
+	_insert_leaf(root, leaf);
+}
+
+bool DynamicBVH::update(const ID &p_id, const AABB &p_box) {
+	ERR_FAIL_COND_V(!p_id.is_valid(), false);
+	Node *leaf = p_id.node;
+
+	Volume volume;
+	volume.min = p_box.position;
+	volume.max = p_box.position + p_box.size;
+
+	if (leaf->volume.min.is_equal_approx(volume.min) && leaf->volume.max.is_equal_approx(volume.max)) {
+		// noop
+		return false;
+	}
+
+	Node *base = _remove_leaf(leaf);
+	if (base) {
+		if (lkhd >= 0) {
+			for (int i = 0; (i < lkhd) && base->parent; ++i) {
+				base = base->parent;
+			}
+		} else {
+			base = bvh_root;
+		}
+	}
+	leaf->volume = volume;
+	_insert_leaf(base, leaf);
+	return true;
+}
+
+void DynamicBVH::remove(const ID &p_id) {
+	ERR_FAIL_COND(!p_id.is_valid());
+	Node *leaf = p_id.node;
+	_remove_leaf(leaf);
+	_delete_node(leaf);
+	--total_leaves;
+}
+
+void DynamicBVH::_extract_leaves(Node *p_node, List<ID> *r_elements) {
+	if (p_node->is_internal()) {
+		_extract_leaves(p_node->children[0], r_elements);
+		_extract_leaves(p_node->children[1], r_elements);
+	} else {
+		ID id;
+		id.node = p_node;
+		r_elements->push_back(id);
+	}
+}
+
+void DynamicBVH::set_index(uint32_t p_index) {
+	ERR_FAIL_COND(bvh_root != nullptr);
+	index = p_index;
+}
+
+uint32_t DynamicBVH::get_index() const {
+	return index;
+}
+
+void DynamicBVH::get_elements(List<ID> *r_elements) {
+	if (bvh_root) {
+		_extract_leaves(bvh_root, r_elements);
+	}
+}
+
+int DynamicBVH::get_leaf_count() const {
+	return total_leaves;
+}
+int DynamicBVH::get_max_depth() const {
+	if (bvh_root) {
+		int depth = 1;
+		int max_depth = 0;
+		bvh_root->get_max_depth(depth, max_depth);
+		return max_depth;
+	} else {
+		return 0;
+	}
+}
+
+DynamicBVH::~DynamicBVH() {
+	clear();
+}

core/math/dynamic_bvh.h

@@ -0,0 +1,475 @@
+/**************************************************************************/
+/*  dynamic_bvh.h                                                         */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "core/math/aabb.h"
+#include "core/templates/list.h"
+#include "core/templates/local_vector.h"
+#include "core/templates/paged_allocator.h"
+#include "core/typedefs.h"
+
+// Based on bullet Dbvh
+
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+///DynamicBVH implementation by Nathanael Presson
+// The DynamicBVH class implements a fast dynamic bounding volume tree based on axis aligned bounding boxes (aabb tree).
+
+class DynamicBVH {
+	struct Node;
+
+public:
+	struct ID {
+		Node *node = nullptr;
+
+	public:
+		_FORCE_INLINE_ bool is_valid() const { return node != nullptr; }
+	};
+
+private:
+	struct Volume {
+		Vector3 min, max;
+
+		_FORCE_INLINE_ Vector3 get_center() const { return ((min + max) / 2); }
+		_FORCE_INLINE_ Vector3 get_length() const { return (max - min); }
+
+		_FORCE_INLINE_ bool contains(const Volume &a) const {
+			return ((min.x <= a.min.x) &&
+					(min.y <= a.min.y) &&
+					(min.z <= a.min.z) &&
+					(max.x >= a.max.x) &&
+					(max.y >= a.max.y) &&
+					(max.z >= a.max.z));
+		}
+
+		_FORCE_INLINE_ Volume merge(const Volume &b) const {
+			Volume r;
+			for (int i = 0; i < 3; ++i) {
+				if (min[i] < b.min[i]) {
+					r.min[i] = min[i];
+				} else {
+					r.min[i] = b.min[i];
+				}
+				if (max[i] > b.max[i]) {
+					r.max[i] = max[i];
+				} else {
+					r.max[i] = b.max[i];
+				}
+			}
+			return r;
+		}
+
+		_FORCE_INLINE_ real_t get_size() const {
+			const Vector3 edges = get_length();
+			return (edges.x * edges.y * edges.z +
+					edges.x + edges.y + edges.z);
+		}
+
+		_FORCE_INLINE_ bool is_not_equal_to(const Volume &b) const {
+			return ((min.x != b.min.x) ||
+					(min.y != b.min.y) ||
+					(min.z != b.min.z) ||
+					(max.x != b.max.x) ||
+					(max.y != b.max.y) ||
+					(max.z != b.max.z));
+		}
+
+		_FORCE_INLINE_ real_t get_proximity_to(const Volume &b) const {
+			const Vector3 d = (min + max) - (b.min + b.max);
+			return (Math::abs(d.x) + Math::abs(d.y) + Math::abs(d.z));
+		}
+
+		_FORCE_INLINE_ int select_by_proximity(const Volume &a, const Volume &b) const {
+			return (get_proximity_to(a) < get_proximity_to(b) ? 0 : 1);
+		}
+
+		//
+		_FORCE_INLINE_ bool intersects(const Volume &b) const {
+			return ((min.x <= b.max.x) &&
+					(max.x >= b.min.x) &&
+					(min.y <= b.max.y) &&
+					(max.y >= b.min.y) &&
+					(min.z <= b.max.z) &&
+					(max.z >= b.min.z));
+		}
+
+		_FORCE_INLINE_ bool intersects_convex(const Plane *p_planes, int p_plane_count, const Vector3 *p_points, int p_point_count) const {
+			Vector3 half_extents = (max - min) * 0.5;
+			Vector3 ofs = min + half_extents;
+
+			for (int i = 0; i < p_plane_count; i++) {
+				const Plane &p = p_planes[i];
+				Vector3 point(
+						(p.normal.x > 0) ? -half_extents.x : half_extents.x,
+						(p.normal.y > 0) ? -half_extents.y : half_extents.y,
+						(p.normal.z > 0) ? -half_extents.z : half_extents.z);
+				point += ofs;
+				if (p.is_point_over(point)) {
+					return false;
+				}
+			}
+
+			// Make sure all points in the shape aren't fully separated from the AABB on
+			// each axis.
+			int bad_point_counts_positive[3] = { 0 };
+			int bad_point_counts_negative[3] = { 0 };
+
+			for (int k = 0; k < 3; k++) {
+				for (int i = 0; i < p_point_count; i++) {
+					if (p_points[i].coord[k] > ofs.coord[k] + half_extents.coord[k]) {
+						bad_point_counts_positive[k]++;
+					}
+					if (p_points[i].coord[k] < ofs.coord[k] - half_extents.coord[k]) {
+						bad_point_counts_negative[k]++;
+					}
+				}
+
+				if (bad_point_counts_negative[k] == p_point_count) {
+					return false;
+				}
+				if (bad_point_counts_positive[k] == p_point_count) {
+					return false;
+				}
+			}
+
+			return true;
+		}
+	};
+
+	struct Node {
+		Volume volume;
+		Node *parent = nullptr;
+		union {
+			Node *children[2];
+			void *data;
+		};
+
+		_FORCE_INLINE_ bool is_leaf() const { return children[1] == nullptr; }
+		_FORCE_INLINE_ bool is_internal() const { return (!is_leaf()); }
+
+		_FORCE_INLINE_ int get_index_in_parent() const {
+			ERR_FAIL_NULL_V(parent, 0);
+			return (parent->children[1] == this) ? 1 : 0;
+		}
+		void get_max_depth(int depth, int &maxdepth) {
+			if (is_internal()) {
+				children[0]->get_max_depth(depth + 1, maxdepth);
+				children[1]->get_max_depth(depth + 1, maxdepth);
+			} else {
+				maxdepth = MAX(maxdepth, depth);
+			}
+		}
+
+		//
+		int count_leaves() const {
+			if (is_internal()) {
+				return children[0]->count_leaves() + children[1]->count_leaves();
+			} else {
+				return (1);
+			}
+		}
+
+		bool is_left_of_axis(const Vector3 &org, const Vector3 &axis) const {
+			return axis.dot(volume.get_center() - org) <= 0;
+		}
+
+		Node() {
+			children[0] = nullptr;
+			children[1] = nullptr;
+		}
+	};
+
+	PagedAllocator<Node> node_allocator;
+	// Fields
+	Node *bvh_root = nullptr;
+	int lkhd = -1;
+	int total_leaves = 0;
+	uint32_t opath = 0;
+	uint32_t index = 0;
+
+	enum {
+		ALLOCA_STACK_SIZE = 128
+	};
+
+	_FORCE_INLINE_ void _delete_node(Node *p_node);
+	void _recurse_delete_node(Node *p_node);
+	_FORCE_INLINE_ Node *_create_node(Node *p_parent, void *p_data);
+	_FORCE_INLINE_ DynamicBVH::Node *_create_node_with_volume(Node *p_parent, const Volume &p_volume, void *p_data);
+	_FORCE_INLINE_ void _insert_leaf(Node *p_root, Node *p_leaf);
+	_FORCE_INLINE_ Node *_remove_leaf(Node *leaf);
+	void _fetch_leaves(Node *p_root, LocalVector<Node *> &r_leaves, int p_depth = -1);
+	static int _split(Node **leaves, int p_count, const Vector3 &p_org, const Vector3 &p_axis);
+	static Volume _bounds(Node **leaves, int p_count);
+	void _bottom_up(Node **leaves, int p_count);
+	Node *_top_down(Node **leaves, int p_count, int p_bu_threshold);
+	Node *_node_sort(Node *n, Node *&r);
+
+	_FORCE_INLINE_ void _update(Node *leaf, int lookahead = -1);
+
+	void _extract_leaves(Node *p_node, List<ID> *r_elements);
+
+	_FORCE_INLINE_ bool _ray_aabb(const Vector3 &rayFrom, const Vector3 &rayInvDirection, const unsigned int raySign[3], const Vector3 bounds[2], real_t &tmin, real_t lambda_min, real_t lambda_max) {
+		real_t tmax, tymin, tymax, tzmin, tzmax;
+		tmin = (bounds[raySign[0]].x - rayFrom.x) * rayInvDirection.x;
+		tmax = (bounds[1 - raySign[0]].x - rayFrom.x) * rayInvDirection.x;
+		tymin = (bounds[raySign[1]].y - rayFrom.y) * rayInvDirection.y;
+		tymax = (bounds[1 - raySign[1]].y - rayFrom.y) * rayInvDirection.y;
+
+		if ((tmin > tymax) || (tymin > tmax)) {
+			return false;
+		}
+
+		if (tymin > tmin) {
+			tmin = tymin;
+		}
+
+		if (tymax < tmax) {
+			tmax = tymax;
+		}
+
+		tzmin = (bounds[raySign[2]].z - rayFrom.z) * rayInvDirection.z;
+		tzmax = (bounds[1 - raySign[2]].z - rayFrom.z) * rayInvDirection.z;
+
+		if ((tmin > tzmax) || (tzmin > tmax)) {
+			return false;
+		}
+		if (tzmin > tmin) {
+			tmin = tzmin;
+		}
+		if (tzmax < tmax) {
+			tmax = tzmax;
+		}
+		return ((tmin < lambda_max) && (tmax > lambda_min));
+	}
+
+public:
+	// Methods
+	void clear();
+	bool is_empty() const { return (nullptr == bvh_root); }
+	void optimize_bottom_up();
+	void optimize_top_down(int bu_threshold = 128);
+	void optimize_incremental(int passes);
+	ID insert(const AABB &p_box, void *p_userdata);
+	bool update(const ID &p_id, const AABB &p_box);
+	void remove(const ID &p_id);
+	void get_elements(List<ID> *r_elements);
+
+	int get_leaf_count() const;
+	int get_max_depth() const;
+
+	/* Discouraged, but works as a reference on how it must be used */
+	struct DefaultQueryResult {
+		virtual bool operator()(void *p_data) = 0; //return true whether you want to continue the query
+		virtual ~DefaultQueryResult() {}
+	};
+
+	template <typename QueryResult>
+	_FORCE_INLINE_ void aabb_query(const AABB &p_aabb, QueryResult &r_result);
+	template <typename QueryResult>
+	_FORCE_INLINE_ void convex_query(const Plane *p_planes, int p_plane_count, const Vector3 *p_points, int p_point_count, QueryResult &r_result);
+	template <typename QueryResult>
+	_FORCE_INLINE_ void ray_query(const Vector3 &p_from, const Vector3 &p_to, QueryResult &r_result);
+
+	void set_index(uint32_t p_index);
+	uint32_t get_index() const;
+
+	~DynamicBVH();
+};
+
+template <typename QueryResult>
+void DynamicBVH::aabb_query(const AABB &p_box, QueryResult &r_result) {
+	if (!bvh_root) {
+		return;
+	}
+
+	Volume volume;
+	volume.min = p_box.position;
+	volume.max = p_box.position + p_box.size;
+
+	const Node **alloca_stack = (const Node **)alloca(ALLOCA_STACK_SIZE * sizeof(const Node *));
+	const Node **stack = alloca_stack;
+	stack[0] = bvh_root;
+	int32_t depth = 1;
+	int32_t threshold = ALLOCA_STACK_SIZE - 2;
+
+	LocalVector<const Node *> aux_stack; //only used in rare occasions when you run out of alloca memory because tree is too unbalanced. Should correct itself over time.
+
+	do {
+		depth--;
+		const Node *n = stack[depth];
+		if (n->volume.intersects(volume)) {
+			if (n->is_internal()) {
+				if (depth > threshold) {
+					if (aux_stack.is_empty()) {
+						aux_stack.resize(ALLOCA_STACK_SIZE * 2);
+						memcpy(aux_stack.ptr(), alloca_stack, ALLOCA_STACK_SIZE * sizeof(const Node *));
+						alloca_stack = nullptr;
+					} else {
+						aux_stack.resize(aux_stack.size() * 2);
+					}
+					stack = aux_stack.ptr();
+					threshold = aux_stack.size() - 2;
+				}
+				stack[depth++] = n->children[0];
+				stack[depth++] = n->children[1];
+			} else {
+				if (r_result(n->data)) {
+					return;
+				}
+			}
+		}
+	} while (depth > 0);
+}
+
+template <typename QueryResult>
+void DynamicBVH::convex_query(const Plane *p_planes, int p_plane_count, const Vector3 *p_points, int p_point_count, QueryResult &r_result) {
+	if (!bvh_root) {
+		return;
+	}
+
+	//generate a volume anyway to improve pre-testing
+	Volume volume;
+	for (int i = 0; i < p_point_count; i++) {
+		if (i == 0) {
+			volume.min = p_points[0];
+			volume.max = p_points[0];
+		} else {
+			volume.min = volume.min.min(p_points[i]);
+			volume.max = volume.max.max(p_points[i]);
+		}
+	}
+
+	const Node **alloca_stack = (const Node **)alloca(ALLOCA_STACK_SIZE * sizeof(const Node *));
+	const Node **stack = alloca_stack;
+	stack[0] = bvh_root;
+	int32_t depth = 1;
+	int32_t threshold = ALLOCA_STACK_SIZE - 2;
+
+	LocalVector<const Node *> aux_stack; //only used in rare occasions when you run out of alloca memory because tree is too unbalanced. Should correct itself over time.
+
+	do {
+		depth--;
+		const Node *n = stack[depth];
+		if (n->volume.intersects(volume) && n->volume.intersects_convex(p_planes, p_plane_count, p_points, p_point_count)) {
+			if (n->is_internal()) {
+				if (depth > threshold) {
+					if (aux_stack.is_empty()) {
+						aux_stack.resize(ALLOCA_STACK_SIZE * 2);
+						memcpy(aux_stack.ptr(), alloca_stack, ALLOCA_STACK_SIZE * sizeof(const Node *));
+						alloca_stack = nullptr;
+					} else {
+						aux_stack.resize(aux_stack.size() * 2);
+					}
+					stack = aux_stack.ptr();
+					threshold = aux_stack.size() - 2;
+				}
+				stack[depth++] = n->children[0];
+				stack[depth++] = n->children[1];
+			} else {
+				if (r_result(n->data)) {
+					return;
+				}
+			}
+		}
+	} while (depth > 0);
+}
+template <typename QueryResult>
+void DynamicBVH::ray_query(const Vector3 &p_from, const Vector3 &p_to, QueryResult &r_result) {
+	if (!bvh_root) {
+		return;
+	}
+
+	Vector3 ray_dir = (p_to - p_from);
+	ray_dir.normalize();
+
+	///what about division by zero? --> just set rayDirection[i] to INF/B3_LARGE_FLOAT
+	Vector3 inv_dir;
+	inv_dir[0] = ray_dir[0] == real_t(0.0) ? real_t(1e20) : real_t(1.0) / ray_dir[0];
+	inv_dir[1] = ray_dir[1] == real_t(0.0) ? real_t(1e20) : real_t(1.0) / ray_dir[1];
+	inv_dir[2] = ray_dir[2] == real_t(0.0) ? real_t(1e20) : real_t(1.0) / ray_dir[2];
+	unsigned int signs[3] = { inv_dir[0] < 0.0, inv_dir[1] < 0.0, inv_dir[2] < 0.0 };
+
+	real_t lambda_max = ray_dir.dot(p_to - p_from);
+
+	Vector3 bounds[2];
+
+	const Node **alloca_stack = (const Node **)alloca(ALLOCA_STACK_SIZE * sizeof(const Node *));
+	const Node **stack = alloca_stack;
+	stack[0] = bvh_root;
+	int32_t depth = 1;
+	int32_t threshold = ALLOCA_STACK_SIZE - 2;
+
+	LocalVector<const Node *> aux_stack; //only used in rare occasions when you run out of alloca memory because tree is too unbalanced. Should correct itself over time.
+
+	do {
+		depth--;
+		const Node *node = stack[depth];
+		bounds[0] = node->volume.min;
+		bounds[1] = node->volume.max;
+		real_t tmin = 1.f, lambda_min = 0.f;
+		unsigned int result1 = false;
+		result1 = _ray_aabb(p_from, inv_dir, signs, bounds, tmin, lambda_min, lambda_max);
+		if (result1) {
+			if (node->is_internal()) {
+				if (depth > threshold) {
+					if (aux_stack.is_empty()) {
+						aux_stack.resize(ALLOCA_STACK_SIZE * 2);
+						memcpy(aux_stack.ptr(), alloca_stack, ALLOCA_STACK_SIZE * sizeof(const Node *));
+						alloca_stack = nullptr;
+					} else {
+						aux_stack.resize(aux_stack.size() * 2);
+					}
+					stack = aux_stack.ptr();
+					threshold = aux_stack.size() - 2;
+				}
+				stack[depth++] = node->children[0];
+				stack[depth++] = node->children[1];
+			} else {
+				if (r_result(node->data)) {
+					return;
+				}
+			}
+		}
+	} while (depth > 0);
+}

core/math/expression.h

@@ -0,0 +1,272 @@
+/**************************************************************************/
+/*  expression.h                                                          */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "core/object/ref_counted.h"
+
+class Expression : public RefCounted {
+	GDCLASS(Expression, RefCounted);
+
+private:
+	struct Input {
+		Variant::Type type = Variant::NIL;
+		String name;
+
+		Input() {}
+	};
+
+	Vector<Input> inputs;
+	Variant::Type output_type = Variant::NIL;
+
+	String expression;
+
+	bool sequenced = false;
+	int str_ofs = 0;
+	bool expression_dirty = false;
+
+	bool _compile_expression();
+
+	enum TokenType {
+		TK_CURLY_BRACKET_OPEN,
+		TK_CURLY_BRACKET_CLOSE,
+		TK_BRACKET_OPEN,
+		TK_BRACKET_CLOSE,
+		TK_PARENTHESIS_OPEN,
+		TK_PARENTHESIS_CLOSE,
+		TK_IDENTIFIER,
+		TK_BUILTIN_FUNC,
+		TK_SELF,
+		TK_CONSTANT,
+		TK_BASIC_TYPE,
+		TK_COLON,
+		TK_COMMA,
+		TK_PERIOD,
+		TK_OP_IN,
+		TK_OP_EQUAL,
+		TK_OP_NOT_EQUAL,
+		TK_OP_LESS,
+		TK_OP_LESS_EQUAL,
+		TK_OP_GREATER,
+		TK_OP_GREATER_EQUAL,
+		TK_OP_AND,
+		TK_OP_OR,
+		TK_OP_NOT,
+		TK_OP_ADD,
+		TK_OP_SUB,
+		TK_OP_MUL,
+		TK_OP_DIV,
+		TK_OP_MOD,
+		TK_OP_POW,
+		TK_OP_SHIFT_LEFT,
+		TK_OP_SHIFT_RIGHT,
+		TK_OP_BIT_AND,
+		TK_OP_BIT_OR,
+		TK_OP_BIT_XOR,
+		TK_OP_BIT_INVERT,
+		TK_INPUT,
+		TK_EOF,
+		TK_ERROR,
+		TK_MAX
+	};
+
+	static const char *token_name[TK_MAX];
+	struct Token {
+		TokenType type;
+		Variant value;
+	};
+
+	void _set_error(const String &p_err) {
+		if (error_set) {
+			return;
+		}
+		error_str = p_err;
+		error_set = true;
+	}
+
+	Error _get_token(Token &r_token);
+
+	String error_str;
+	bool error_set = true;
+
+	struct ENode {
+		enum Type {
+			TYPE_INPUT,
+			TYPE_CONSTANT,
+			TYPE_SELF,
+			TYPE_OPERATOR,
+			TYPE_INDEX,
+			TYPE_NAMED_INDEX,
+			TYPE_ARRAY,
+			TYPE_DICTIONARY,
+			TYPE_CONSTRUCTOR,
+			TYPE_BUILTIN_FUNC,
+			TYPE_CALL
+		};
+
+		ENode *next = nullptr;
+
+		Type type = TYPE_INPUT;
+
+		ENode() {}
+		virtual ~ENode() {
+			if (next) {
+				memdelete(next);
+			}
+		}
+	};
+
+	struct ExpressionNode {
+		bool is_op = false;
+		union {
+			Variant::Operator op;
+			ENode *node = nullptr;
+		};
+	};
+
+	ENode *_parse_expression();
+
+	struct InputNode : public ENode {
+		int index = 0;
+		InputNode() {
+			type = TYPE_INPUT;
+		}
+	};
+
+	struct ConstantNode : public ENode {
+		Variant value = Variant::NIL;
+		ConstantNode() {
+			type = TYPE_CONSTANT;
+		}
+	};
+
+	struct OperatorNode : public ENode {
+		Variant::Operator op = Variant::Operator::OP_ADD;
+
+		ENode *nodes[2] = { nullptr, nullptr };
+
+		OperatorNode() {
+			type = TYPE_OPERATOR;
+		}
+	};
+
+	struct SelfNode : public ENode {
+		SelfNode() {
+			type = TYPE_SELF;
+		}
+	};
+
+	struct IndexNode : public ENode {
+		ENode *base = nullptr;
+		ENode *index = nullptr;
+
+		IndexNode() {
+			type = TYPE_INDEX;
+		}
+	};
+
+	struct NamedIndexNode : public ENode {
+		ENode *base = nullptr;
+		StringName name;
+
+		NamedIndexNode() {
+			type = TYPE_NAMED_INDEX;
+		}
+	};
+
+	struct ConstructorNode : public ENode {
+		Variant::Type data_type = Variant::Type::NIL;
+		Vector<ENode *> arguments;
+
+		ConstructorNode() {
+			type = TYPE_CONSTRUCTOR;
+		}
+	};
+
+	struct CallNode : public ENode {
+		ENode *base = nullptr;
+		StringName method;
+		Vector<ENode *> arguments;
+
+		CallNode() {
+			type = TYPE_CALL;
+		}
+	};
+
+	struct ArrayNode : public ENode {
+		Vector<ENode *> array;
+		ArrayNode() {
+			type = TYPE_ARRAY;
+		}
+	};
+
+	struct DictionaryNode : public ENode {
+		Vector<ENode *> dict;
+		DictionaryNode() {
+			type = TYPE_DICTIONARY;
+		}
+	};
+
+	struct BuiltinFuncNode : public ENode {
+		StringName func;
+		Vector<ENode *> arguments;
+		BuiltinFuncNode() {
+			type = TYPE_BUILTIN_FUNC;
+		}
+	};
+
+	template <typename T>
+	T *alloc_node() {
+		T *node = memnew(T);
+		node->next = nodes;
+		nodes = node;
+		return node;
+	}
+
+	ENode *root = nullptr;
+	ENode *nodes = nullptr;
+
+	Vector<String> input_names;
+
+	bool execution_error = false;
+	bool _execute(const Array &p_inputs, Object *p_instance, Expression::ENode *p_node, Variant &r_ret, bool p_const_calls_only, String &r_error_str);
+
+protected:
+	static void _bind_methods();
+
+public:
+	Error parse(const String &p_expression, const Vector<String> &p_input_names = Vector<String>());
+	Variant execute(const Array &p_inputs = Array(), Object *p_base = nullptr, bool p_show_error = true, bool p_const_calls_only = false);
+	bool has_execute_failed() const;
+	String get_error_text() const;
+
+	Expression() {}
+	~Expression();
+};

core/math/face3.cpp

@@ -0,0 +1,351 @@
+/**************************************************************************/
+/*  face3.cpp                                                             */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "face3.h"
+
+#include "core/math/geometry_3d.h"
+
+int Face3::split_by_plane(const Plane &p_plane, Face3 p_res[3], bool p_is_point_over[3]) const {
+	ERR_FAIL_COND_V(is_degenerate(), 0);
+
+	Vector3 above[4];
+	int above_count = 0;
+
+	Vector3 below[4];
+	int below_count = 0;
+
+	for (int i = 0; i < 3; i++) {
+		if (p_plane.has_point(vertex[i], (real_t)CMP_EPSILON)) { // point is in plane
+
+			ERR_FAIL_COND_V(above_count >= 4, 0);
+			above[above_count++] = vertex[i];
+			ERR_FAIL_COND_V(below_count >= 4, 0);
+			below[below_count++] = vertex[i];
+
+		} else {
+			if (p_plane.is_point_over(vertex[i])) {
+				//Point is over
+				ERR_FAIL_COND_V(above_count >= 4, 0);
+				above[above_count++] = vertex[i];
+
+			} else {
+				//Point is under
+				ERR_FAIL_COND_V(below_count >= 4, 0);
+				below[below_count++] = vertex[i];
+			}
+
+			/* Check for Intersection between this and the next vertex*/
+
+			Vector3 inters;
+			if (!p_plane.intersects_segment(vertex[i], vertex[(i + 1) % 3], &inters)) {
+				continue;
+			}
+
+			/* Intersection goes to both */
+			ERR_FAIL_COND_V(above_count >= 4, 0);
+			above[above_count++] = inters;
+			ERR_FAIL_COND_V(below_count >= 4, 0);
+			below[below_count++] = inters;
+		}
+	}
+
+	int polygons_created = 0;
+
+	ERR_FAIL_COND_V(above_count >= 4 && below_count >= 4, 0); //bug in the algo
+
+	if (above_count >= 3) {
+		p_res[polygons_created] = Face3(above[0], above[1], above[2]);
+		p_is_point_over[polygons_created] = true;
+		polygons_created++;
+
+		if (above_count == 4) {
+			p_res[polygons_created] = Face3(above[2], above[3], above[0]);
+			p_is_point_over[polygons_created] = true;
+			polygons_created++;
+		}
+	}
+
+	if (below_count >= 3) {
+		p_res[polygons_created] = Face3(below[0], below[1], below[2]);
+		p_is_point_over[polygons_created] = false;
+		polygons_created++;
+
+		if (below_count == 4) {
+			p_res[polygons_created] = Face3(below[2], below[3], below[0]);
+			p_is_point_over[polygons_created] = false;
+			polygons_created++;
+		}
+	}
+
+	return polygons_created;
+}
+
+bool Face3::intersects_ray(const Vector3 &p_from, const Vector3 &p_dir, Vector3 *p_intersection) const {
+	return Geometry3D::ray_intersects_triangle(p_from, p_dir, vertex[0], vertex[1], vertex[2], p_intersection);
+}
+
+bool Face3::intersects_segment(const Vector3 &p_from, const Vector3 &p_dir, Vector3 *p_intersection) const {
+	return Geometry3D::segment_intersects_triangle(p_from, p_dir, vertex[0], vertex[1], vertex[2], p_intersection);
+}
+
+bool Face3::is_degenerate() const {
+	Vector3 normal = vec3_cross(vertex[0] - vertex[1], vertex[0] - vertex[2]);
+	return (normal.length_squared() < (real_t)CMP_EPSILON2);
+}
+
+Vector3 Face3::get_random_point_inside() const {
+	real_t a = Math::random(0.0, 1.0);
+	real_t b = Math::random(0.0, 1.0);
+	if (a > b) {
+		SWAP(a, b);
+	}
+
+	return vertex[0] * a + vertex[1] * (b - a) + vertex[2] * (1.0f - b);
+}
+
+Plane Face3::get_plane(ClockDirection p_dir) const {
+	return Plane(vertex[0], vertex[1], vertex[2], p_dir);
+}
+
+real_t Face3::get_area() const {
+	return vec3_cross(vertex[0] - vertex[1], vertex[0] - vertex[2]).length() * 0.5f;
+}
+
+bool Face3::intersects_aabb(const AABB &p_aabb) const {
+	/** TEST PLANE **/
+	if (!p_aabb.intersects_plane(get_plane())) {
+		return false;
+	}
+
+#define TEST_AXIS(m_ax)                                            \
+	/** TEST FACE AXIS */                                          \
+	{                                                              \
+		real_t aabb_min = p_aabb.position.m_ax;                    \
+		real_t aabb_max = p_aabb.position.m_ax + p_aabb.size.m_ax; \
+		real_t tri_min = vertex[0].m_ax;                           \
+		real_t tri_max = vertex[0].m_ax;                           \
+		for (int i = 1; i < 3; i++) {                              \
+			if (vertex[i].m_ax > tri_max)                          \
+				tri_max = vertex[i].m_ax;                          \
+			if (vertex[i].m_ax < tri_min)                          \
+				tri_min = vertex[i].m_ax;                          \
+		}                                                          \
+                                                                   \
+		if (tri_max < aabb_min || aabb_max < tri_min)              \
+			return false;                                          \
+	}
+
+	TEST_AXIS(x);
+	TEST_AXIS(y);
+	TEST_AXIS(z);
+
+	/** TEST ALL EDGES **/
+
+	const Vector3 edge_norms[3] = {
+		vertex[0] - vertex[1],
+		vertex[1] - vertex[2],
+		vertex[2] - vertex[0],
+	};
+
+	for (int i = 0; i < 12; i++) {
+		Vector3 from, to;
+		p_aabb.get_edge(i, from, to);
+		Vector3 e1 = from - to;
+		for (int j = 0; j < 3; j++) {
+			Vector3 e2 = edge_norms[j];
+
+			Vector3 axis = vec3_cross(e1, e2);
+
+			if (axis.length_squared() < 0.0001f) {
+				continue; // coplanar
+			}
+			axis.normalize();
+
+			real_t minA, maxA, minB, maxB;
+			p_aabb.project_range_in_plane(Plane(axis), minA, maxA);
+			project_range(axis, Transform3D(), minB, maxB);
+
+			if (maxA < minB || maxB < minA) {
+				return false;
+			}
+		}
+	}
+	return true;
+}
+
+Face3::operator String() const {
+	return String() + String(vertex[0]) + ", " + String(vertex[1]) + ", " + String(vertex[2]);
+}
+
+void Face3::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const {
+	for (int i = 0; i < 3; i++) {
+		Vector3 v = p_transform.xform(vertex[i]);
+		real_t d = p_normal.dot(v);
+
+		if (i == 0 || d > r_max) {
+			r_max = d;
+		}
+
+		if (i == 0 || d < r_min) {
+			r_min = d;
+		}
+	}
+}
+
+void Face3::get_support(const Vector3 &p_normal, const Transform3D &p_transform, Vector3 *p_vertices, int *p_count, int p_max) const {
+	constexpr double face_support_threshold = 0.98;
+	constexpr double edge_support_threshold = 0.05;
+
+	if (p_max <= 0) {
+		return;
+	}
+
+	Vector3 n = p_transform.basis.xform_inv(p_normal);
+
+	/** TEST FACE AS SUPPORT **/
+	if (get_plane().normal.dot(n) > face_support_threshold) {
+		*p_count = MIN(3, p_max);
+
+		for (int i = 0; i < *p_count; i++) {
+			p_vertices[i] = p_transform.xform(vertex[i]);
+		}
+
+		return;
+	}
+
+	/** FIND SUPPORT VERTEX **/
+
+	int vert_support_idx = -1;
+	real_t support_max = 0;
+
+	for (int i = 0; i < 3; i++) {
+		real_t d = n.dot(vertex[i]);
+
+		if (i == 0 || d > support_max) {
+			support_max = d;
+			vert_support_idx = i;
+		}
+	}
+
+	/** TEST EDGES AS SUPPORT **/
+
+	for (int i = 0; i < 3; i++) {
+		if (i != vert_support_idx && i + 1 != vert_support_idx) {
+			continue;
+		}
+
+		// check if edge is valid as a support
+		real_t dot = (vertex[i] - vertex[(i + 1) % 3]).normalized().dot(n);
+		dot = Math::abs(dot);
+		if (dot < edge_support_threshold) {
+			*p_count = MIN(2, p_max);
+
+			for (int j = 0; j < *p_count; j++) {
+				p_vertices[j] = p_transform.xform(vertex[(j + i) % 3]);
+			}
+
+			return;
+		}
+	}
+
+	*p_count = 1;
+	p_vertices[0] = p_transform.xform(vertex[vert_support_idx]);
+}
+
+Vector3 Face3::get_closest_point_to(const Vector3 &p_point) const {
+	Vector3 edge0 = vertex[1] - vertex[0];
+	Vector3 edge1 = vertex[2] - vertex[0];
+	Vector3 v0 = vertex[0] - p_point;
+
+	real_t a = edge0.dot(edge0);
+	real_t b = edge0.dot(edge1);
+	real_t c = edge1.dot(edge1);
+	real_t d = edge0.dot(v0);
+	real_t e = edge1.dot(v0);
+
+	real_t det = a * c - b * b;
+	real_t s = b * e - c * d;
+	real_t t = b * d - a * e;
+
+	if (s + t < det) {
+		if (s < 0.f) {
+			if (t < 0.f) {
+				if (d < 0.f) {
+					s = CLAMP(-d / a, 0.f, 1.f);
+					t = 0.f;
+				} else {
+					s = 0.f;
+					t = CLAMP(-e / c, 0.f, 1.f);
+				}
+			} else {
+				s = 0.f;
+				t = CLAMP(-e / c, 0.f, 1.f);
+			}
+		} else if (t < 0.f) {
+			s = CLAMP(-d / a, 0.f, 1.f);
+			t = 0.f;
+		} else {
+			real_t invDet = 1.f / det;
+			s *= invDet;
+			t *= invDet;
+		}
+	} else {
+		if (s < 0.f) {
+			real_t tmp0 = b + d;
+			real_t tmp1 = c + e;
+			if (tmp1 > tmp0) {
+				real_t numer = tmp1 - tmp0;
+				real_t denom = a - 2 * b + c;
+				s = CLAMP(numer / denom, 0.f, 1.f);
+				t = 1 - s;
+			} else {
+				t = CLAMP(-e / c, 0.f, 1.f);
+				s = 0.f;
+			}
+		} else if (t < 0.f) {
+			if (a + d > b + e) {
+				real_t numer = c + e - b - d;
+				real_t denom = a - 2 * b + c;
+				s = CLAMP(numer / denom, 0.f, 1.f);
+				t = 1 - s;
+			} else {
+				s = CLAMP(-d / a, 0.f, 1.f);
+				t = 0.f;
+			}
+		} else {
+			real_t numer = c + e - b - d;
+			real_t denom = a - 2 * b + c;
+			s = CLAMP(numer / denom, 0.f, 1.f);
+			t = 1.f - s;
+		}
+	}
+
+	return vertex[0] + s * edge0 + t * edge1;
+}

core/math/face3.h

@@ -0,0 +1,238 @@
+/**************************************************************************/
+/*  face3.h                                                               */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "core/math/aabb.h"
+#include "core/math/plane.h"
+#include "core/math/transform_3d.h"
+#include "core/math/vector3.h"
+
+struct [[nodiscard]] Face3 {
+	enum Side {
+		SIDE_OVER,
+		SIDE_UNDER,
+		SIDE_SPANNING,
+		SIDE_COPLANAR
+	};
+
+	Vector3 vertex[3];
+
+	/**
+	 * @param p_plane plane used to split the face
+	 * @param p_res array of at least 3 faces, amount used in function return
+	 * @param p_is_point_over array of at least 3 booleans, determining which face is over the plane, amount used in function return
+	 * @return amount of faces generated by the split, either 0 (means no split possible), 2 or 3
+	 */
+	int split_by_plane(const Plane &p_plane, Face3 *p_res, bool *p_is_point_over) const;
+
+	Plane get_plane(ClockDirection p_dir = CLOCKWISE) const;
+	Vector3 get_random_point_inside() const;
+
+	bool is_degenerate() const;
+	real_t get_area() const;
+
+	Vector3 get_closest_point_to(const Vector3 &p_point) const;
+
+	bool intersects_ray(const Vector3 &p_from, const Vector3 &p_dir, Vector3 *p_intersection = nullptr) const;
+	bool intersects_segment(const Vector3 &p_from, const Vector3 &p_dir, Vector3 *p_intersection = nullptr) const;
+
+	void get_support(const Vector3 &p_normal, const Transform3D &p_transform, Vector3 *p_vertices, int *p_count, int p_max) const;
+	void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const;
+
+	AABB get_aabb() const {
+		AABB aabb(vertex[0], Vector3());
+		aabb.expand_to(vertex[1]);
+		aabb.expand_to(vertex[2]);
+		return aabb;
+	}
+
+	bool intersects_aabb(const AABB &p_aabb) const;
+	_FORCE_INLINE_ bool intersects_aabb2(const AABB &p_aabb) const;
+	explicit operator String() const;
+
+	Face3() = default;
+	constexpr Face3(const Vector3 &p_v1, const Vector3 &p_v2, const Vector3 &p_v3) :
+			vertex{ p_v1, p_v2, p_v3 } {}
+};
+
+bool Face3::intersects_aabb2(const AABB &p_aabb) const {
+	Vector3 perp = (vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]);
+
+	Vector3 half_extents = p_aabb.size * 0.5f;
+	Vector3 ofs = p_aabb.position + half_extents;
+
+	Vector3 sup = Vector3(
+			(perp.x > 0) ? -half_extents.x : half_extents.x,
+			(perp.y > 0) ? -half_extents.y : half_extents.y,
+			(perp.z > 0) ? -half_extents.z : half_extents.z);
+
+	real_t d = perp.dot(vertex[0]);
+	real_t dist_a = perp.dot(ofs + sup) - d;
+	real_t dist_b = perp.dot(ofs - sup) - d;
+
+	if (dist_a * dist_b > 0) {
+		return false; //does not intersect the plane
+	}
+
+#define TEST_AXIS(m_ax)                                            \
+	{                                                              \
+		real_t aabb_min = p_aabb.position.m_ax;                    \
+		real_t aabb_max = p_aabb.position.m_ax + p_aabb.size.m_ax; \
+		real_t tri_min, tri_max;                                   \
+		for (int i = 0; i < 3; i++) {                              \
+			if (i == 0 || vertex[i].m_ax > tri_max)                \
+				tri_max = vertex[i].m_ax;                          \
+			if (i == 0 || vertex[i].m_ax < tri_min)                \
+				tri_min = vertex[i].m_ax;                          \
+		}                                                          \
+                                                                   \
+		if (tri_max < aabb_min || aabb_max < tri_min)              \
+			return false;                                          \
+	}
+
+	TEST_AXIS(x);
+	TEST_AXIS(y);
+	TEST_AXIS(z);
+
+#undef TEST_AXIS
+
+	const Vector3 edge_norms[3] = {
+		vertex[0] - vertex[1],
+		vertex[1] - vertex[2],
+		vertex[2] - vertex[0],
+	};
+
+	for (int i = 0; i < 12; i++) {
+		Vector3 from, to;
+		switch (i) {
+			case 0: {
+				from = Vector3(p_aabb.position.x + p_aabb.size.x, p_aabb.position.y, p_aabb.position.z);
+				to = Vector3(p_aabb.position.x, p_aabb.position.y, p_aabb.position.z);
+			} break;
+			case 1: {
+				from = Vector3(p_aabb.position.x + p_aabb.size.x, p_aabb.position.y, p_aabb.position.z + p_aabb.size.z);
+				to = Vector3(p_aabb.position.x + p_aabb.size.x, p_aabb.position.y, p_aabb.position.z);
+			} break;
+			case 2: {
+				from = Vector3(p_aabb.position.x, p_aabb.position.y, p_aabb.position.z + p_aabb.size.z);
+				to = Vector3(p_aabb.position.x + p_aabb.size.x, p_aabb.position.y, p_aabb.position.z + p_aabb.size.z);
+
+			} break;
+			case 3: {
+				from = Vector3(p_aabb.position.x, p_aabb.position.y, p_aabb.position.z);
+				to = Vector3(p_aabb.position.x, p_aabb.position.y, p_aabb.position.z + p_aabb.size.z);
+
+			} break;
+			case 4: {
+				from = Vector3(p_aabb.position.x, p_aabb.position.y + p_aabb.size.y, p_aabb.position.z);
+				to = Vector3(p_aabb.position.x + p_aabb.size.x, p_aabb.position.y + p_aabb.size.y, p_aabb.position.z);
+			} break;
+			case 5: {
+				from = Vector3(p_aabb.position.x + p_aabb.size.x, p_aabb.position.y + p_aabb.size.y, p_aabb.position.z);
+				to = Vector3(p_aabb.position.x + p_aabb.size.x, p_aabb.position.y + p_aabb.size.y, p_aabb.position.z + p_aabb.size.z);
+			} break;
+			case 6: {
+				from = Vector3(p_aabb.position.x + p_aabb.size.x, p_aabb.position.y + p_aabb.size.y, p_aabb.position.z + p_aabb.size.z);
+				to = Vector3(p_aabb.position.x, p_aabb.position.y + p_aabb.size.y, p_aabb.position.z + p_aabb.size.z);
+
+			} break;
+			case 7: {
+				from = Vector3(p_aabb.position.x, p_aabb.position.y + p_aabb.size.y, p_aabb.position.z + p_aabb.size.z);
+				to = Vector3(p_aabb.position.x, p_aabb.position.y + p_aabb.size.y, p_aabb.position.z);
+
+			} break;
+			case 8: {
+				from = Vector3(p_aabb.position.x, p_aabb.position.y, p_aabb.position.z + p_aabb.size.z);
+				to = Vector3(p_aabb.position.x, p_aabb.position.y + p_aabb.size.y, p_aabb.position.z + p_aabb.size.z);
+
+			} break;
+			case 9: {
+				from = Vector3(p_aabb.position.x, p_aabb.position.y, p_aabb.position.z);
+				to = Vector3(p_aabb.position.x, p_aabb.position.y + p_aabb.size.y, p_aabb.position.z);
+
+			} break;
+			case 10: {
+				from = Vector3(p_aabb.position.x + p_aabb.size.x, p_aabb.position.y, p_aabb.position.z);
+				to = Vector3(p_aabb.position.x + p_aabb.size.x, p_aabb.position.y + p_aabb.size.y, p_aabb.position.z);
+
+			} break;
+			case 11: {
+				from = Vector3(p_aabb.position.x + p_aabb.size.x, p_aabb.position.y, p_aabb.position.z + p_aabb.size.z);
+				to = Vector3(p_aabb.position.x + p_aabb.size.x, p_aabb.position.y + p_aabb.size.y, p_aabb.position.z + p_aabb.size.z);
+
+			} break;
+		}
+
+		Vector3 e1 = from - to;
+		for (int j = 0; j < 3; j++) {
+			Vector3 e2 = edge_norms[j];
+
+			Vector3 axis = vec3_cross(e1, e2);
+
+			if (axis.length_squared() < 0.0001f) {
+				continue; // coplanar
+			}
+			//axis.normalize();
+
+			Vector3 sup2 = Vector3(
+					(axis.x > 0) ? -half_extents.x : half_extents.x,
+					(axis.y > 0) ? -half_extents.y : half_extents.y,
+					(axis.z > 0) ? -half_extents.z : half_extents.z);
+
+			real_t maxB = axis.dot(ofs + sup2);
+			real_t minB = axis.dot(ofs - sup2);
+			if (minB > maxB) {
+				SWAP(maxB, minB);
+			}
+
+			real_t minT = 1e20, maxT = -1e20;
+			for (int k = 0; k < 3; k++) {
+				real_t vert_d = axis.dot(vertex[k]);
+
+				if (vert_d > maxT) {
+					maxT = vert_d;
+				}
+
+				if (vert_d < minT) {
+					minT = vert_d;
+				}
+			}
+
+			if (maxB < minT || maxT < minB) {
+				return false;
+			}
+		}
+	}
+	return true;
+}
+
+template <>
+struct is_zero_constructible<Face3> : std::true_type {};

core/math/geometry_2d.cpp

@@ -0,0 +1,398 @@
+/**************************************************************************/
+/*  geometry_2d.cpp                                                       */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "geometry_2d.h"
+
+#include "thirdparty/clipper2/include/clipper2/clipper.h"
+#include "thirdparty/misc/polypartition.h"
+#define STB_RECT_PACK_IMPLEMENTATION
+#include "thirdparty/misc/stb_rect_pack.h"
+
+const int clipper_precision = 5; // Based on CMP_EPSILON.
+const double clipper_scale = Math::pow(10.0, clipper_precision);
+
+void Geometry2D::merge_many_polygons(const Vector<Vector<Vector2>> &p_polygons, Vector<Vector<Vector2>> &r_out_polygons, Vector<Vector<Vector2>> &r_out_holes) {
+	using namespace Clipper2Lib;
+
+	PathsD subjects;
+	for (const Vector<Vector2> &polygon : p_polygons) {
+		PathD path(polygon.size());
+		for (int i = 0; i < polygon.size(); i++) {
+			const Vector2 &point = polygon[i];
+			path[i] = PointD(point.x, point.y);
+		}
+		subjects.push_back(path);
+	}
+
+	PathsD solution = Union(subjects, FillRule::NonZero);
+	solution = SimplifyPaths(solution, 0.01);
+
+	r_out_polygons.clear();
+	r_out_holes.clear();
+	for (PathsD::size_type i = 0; i < solution.size(); ++i) {
+		PathD &path = solution[i];
+
+		Vector<Point2> output_polygon;
+		output_polygon.resize(path.size());
+		for (PathsD::size_type j = 0; j < path.size(); ++j) {
+			output_polygon.set(j, Vector2(static_cast<real_t>(path[j].x), static_cast<real_t>(path[j].y)));
+		}
+		if (IsPositive(path)) {
+			r_out_polygons.push_back(output_polygon);
+		} else {
+			r_out_holes.push_back(output_polygon);
+		}
+	}
+}
+
+Vector<Vector<Vector2>> Geometry2D::decompose_many_polygons_in_convex(const Vector<Vector<Point2>> &p_polygons, const Vector<Vector<Point2>> &p_holes) {
+	Vector<Vector<Vector2>> decomp;
+	List<TPPLPoly> in_poly, out_poly;
+
+	for (const Vector<Vector2> &polygon : p_polygons) {
+		TPPLPoly inp;
+		inp.Init(polygon.size());
+		for (int i = 0; i < polygon.size(); i++) {
+			inp.GetPoint(i) = polygon[i];
+		}
+		inp.SetOrientation(TPPL_ORIENTATION_CCW);
+		in_poly.push_back(inp);
+	}
+	for (const Vector<Vector2> &polygon : p_holes) {
+		TPPLPoly inp;
+		inp.Init(polygon.size());
+		for (int i = 0; i < polygon.size(); i++) {
+			inp.GetPoint(i) = polygon[i];
+		}
+		inp.SetOrientation(TPPL_ORIENTATION_CW);
+		inp.SetHole(true);
+		in_poly.push_back(inp);
+	}
+	TPPLPartition tpart;
+	if (tpart.ConvexPartition_HM(&in_poly, &out_poly) == 0) { // Failed.
+		ERR_PRINT("Convex decomposing failed!");
+		return decomp;
+	}
+
+	decomp.resize(out_poly.size());
+	int idx = 0;
+	for (TPPLPoly &tp : out_poly) {
+		decomp.write[idx].resize(tp.GetNumPoints());
+
+		for (int64_t i = 0; i < tp.GetNumPoints(); i++) {
+			decomp.write[idx].write[i] = tp.GetPoint(i);
+		}
+
+		idx++;
+	}
+
+	return decomp;
+}
+
+Vector<Vector<Vector2>> Geometry2D::decompose_polygon_in_convex(const Vector<Point2> &p_polygon) {
+	return Geometry2D::decompose_many_polygons_in_convex({ p_polygon }, {});
+}
+
+struct _AtlasWorkRect {
+	Size2i s;
+	Point2i p;
+	int idx = 0;
+	_FORCE_INLINE_ bool operator<(const _AtlasWorkRect &p_r) const { return s.width > p_r.s.width; }
+};
+
+struct _AtlasWorkRectResult {
+	Vector<_AtlasWorkRect> result;
+	int max_w = 0;
+	int max_h = 0;
+};
+
+void Geometry2D::make_atlas(const Vector<Size2i> &p_rects, Vector<Point2i> &r_result, Size2i &r_size) {
+	// Super simple, almost brute force scanline stacking fitter.
+	// It's pretty basic for now, but it tries to make sure that the aspect ratio of the
+	// resulting atlas is somehow square. This is necessary because video cards have limits
+	// on texture size (usually 2048 or 4096), so the squarer a texture, the more the chances
+	// that it will work in every hardware.
+	// For example, it will prioritize a 1024x1024 atlas (works everywhere) instead of a
+	// 256x8192 atlas (won't work anywhere).
+
+	ERR_FAIL_COND(p_rects.is_empty());
+	for (int i = 0; i < p_rects.size(); i++) {
+		ERR_FAIL_COND(p_rects[i].width <= 0);
+		ERR_FAIL_COND(p_rects[i].height <= 0);
+	}
+
+	Vector<_AtlasWorkRect> wrects;
+	wrects.resize(p_rects.size());
+	for (int i = 0; i < p_rects.size(); i++) {
+		wrects.write[i].s = p_rects[i];
+		wrects.write[i].idx = i;
+	}
+	wrects.sort();
+	int widest = wrects[0].s.width;
+
+	Vector<_AtlasWorkRectResult> results;
+
+	for (int i = 0; i <= 12; i++) {
+		int w = 1 << i;
+		int max_h = 0;
+		int max_w = 0;
+		if (w < widest) {
+			continue;
+		}
+
+		Vector<int> hmax;
+		hmax.resize(w);
+		for (int j = 0; j < w; j++) {
+			hmax.write[j] = 0;
+		}
+
+		// Place them.
+		int ofs = 0;
+		int limit_h = 0;
+		for (int j = 0; j < wrects.size(); j++) {
+			if (ofs + wrects[j].s.width > w) {
+				ofs = 0;
+			}
+
+			int from_y = 0;
+			for (int k = 0; k < wrects[j].s.width; k++) {
+				if (hmax[ofs + k] > from_y) {
+					from_y = hmax[ofs + k];
+				}
+			}
+
+			wrects.write[j].p.x = ofs;
+			wrects.write[j].p.y = from_y;
+			int end_h = from_y + wrects[j].s.height;
+			int end_w = ofs + wrects[j].s.width;
+			if (ofs == 0) {
+				limit_h = end_h;
+			}
+
+			for (int k = 0; k < wrects[j].s.width; k++) {
+				hmax.write[ofs + k] = end_h;
+			}
+
+			if (end_h > max_h) {
+				max_h = end_h;
+			}
+
+			if (end_w > max_w) {
+				max_w = end_w;
+			}
+
+			if (ofs == 0 || end_h > limit_h) { // While h limit not reached, keep stacking.
+				ofs += wrects[j].s.width;
+			}
+		}
+
+		_AtlasWorkRectResult result;
+		result.result = wrects;
+		result.max_h = max_h;
+		result.max_w = max_w;
+		results.push_back(result);
+	}
+
+	// Find the result with the best aspect ratio.
+
+	int best = -1;
+	real_t best_aspect = 1e20;
+
+	for (int i = 0; i < results.size(); i++) {
+		real_t h = next_power_of_2((uint32_t)results[i].max_h);
+		real_t w = next_power_of_2((uint32_t)results[i].max_w);
+		real_t aspect = h > w ? h / w : w / h;
+		if (aspect < best_aspect) {
+			best = i;
+			best_aspect = aspect;
+		}
+	}
+
+	r_result.resize(p_rects.size());
+
+	for (int i = 0; i < p_rects.size(); i++) {
+		r_result.write[results[best].result[i].idx] = results[best].result[i].p;
+	}
+
+	r_size = Size2(results[best].max_w, results[best].max_h);
+}
+
+Vector<Vector<Point2>> Geometry2D::_polypaths_do_operation(PolyBooleanOperation p_op, const Vector<Point2> &p_polypath_a, const Vector<Point2> &p_polypath_b, bool is_a_open) {
+	using namespace Clipper2Lib;
+
+	ClipType op = ClipType::Union;
+
+	switch (p_op) {
+		case OPERATION_UNION:
+			op = ClipType::Union;
+			break;
+		case OPERATION_DIFFERENCE:
+			op = ClipType::Difference;
+			break;
+		case OPERATION_INTERSECTION:
+			op = ClipType::Intersection;
+			break;
+		case OPERATION_XOR:
+			op = ClipType::Xor;
+			break;
+	}
+
+	PathD path_a(p_polypath_a.size());
+	for (int i = 0; i != p_polypath_a.size(); ++i) {
+		path_a[i] = PointD(p_polypath_a[i].x, p_polypath_a[i].y);
+	}
+	PathD path_b(p_polypath_b.size());
+	for (int i = 0; i != p_polypath_b.size(); ++i) {
+		path_b[i] = PointD(p_polypath_b[i].x, p_polypath_b[i].y);
+	}
+
+	ClipperD clp(clipper_precision); // Scale points up internally to attain the desired precision.
+	clp.PreserveCollinear(false); // Remove redundant vertices.
+	if (is_a_open) {
+		clp.AddOpenSubject({ path_a });
+	} else {
+		clp.AddSubject({ path_a });
+	}
+	clp.AddClip({ path_b });
+
+	PathsD paths;
+
+	if (is_a_open) {
+		PolyTreeD tree; // Needed to populate polylines.
+		clp.Execute(op, FillRule::EvenOdd, tree, paths);
+	} else {
+		clp.Execute(op, FillRule::EvenOdd, paths); // Works on closed polygons only.
+	}
+
+	Vector<Vector<Point2>> polypaths;
+	for (PathsD::size_type i = 0; i < paths.size(); ++i) {
+		const PathD &path = paths[i];
+
+		Vector<Vector2> polypath;
+		for (PathsD::size_type j = 0; j < path.size(); ++j) {
+			polypath.push_back(Point2(static_cast<real_t>(path[j].x), static_cast<real_t>(path[j].y)));
+		}
+		polypaths.push_back(polypath);
+	}
+	return polypaths;
+}
+
+Vector<Vector<Point2>> Geometry2D::_polypath_offset(const Vector<Point2> &p_polypath, real_t p_delta, PolyJoinType p_join_type, PolyEndType p_end_type) {
+	using namespace Clipper2Lib;
+
+	JoinType jt = JoinType::Square;
+
+	switch (p_join_type) {
+		case JOIN_SQUARE:
+			jt = JoinType::Square;
+			break;
+		case JOIN_ROUND:
+			jt = JoinType::Round;
+			break;
+		case JOIN_MITER:
+			jt = JoinType::Miter;
+			break;
+	}
+
+	EndType et = EndType::Polygon;
+
+	switch (p_end_type) {
+		case END_POLYGON:
+			et = EndType::Polygon;
+			break;
+		case END_JOINED:
+			et = EndType::Joined;
+			break;
+		case END_BUTT:
+			et = EndType::Butt;
+			break;
+		case END_SQUARE:
+			et = EndType::Square;
+			break;
+		case END_ROUND:
+			et = EndType::Round;
+			break;
+	}
+
+	PathD polypath(p_polypath.size());
+	for (int i = 0; i != p_polypath.size(); ++i) {
+		polypath[i] = PointD(p_polypath[i].x, p_polypath[i].y);
+	}
+
+	// Inflate/deflate.
+	PathsD paths = InflatePaths({ polypath }, p_delta, jt, et, 2.0, clipper_precision, 0.25 * clipper_scale);
+	// Here the points are scaled up internally and
+	// the arc_tolerance is scaled accordingly
+	// to attain the desired precision.
+
+	Vector<Vector<Point2>> polypaths;
+	for (PathsD::size_type i = 0; i < paths.size(); ++i) {
+		const PathD &path = paths[i];
+
+		Vector<Vector2> polypath2;
+		for (PathsD::size_type j = 0; j < path.size(); ++j) {
+			polypath2.push_back(Point2(static_cast<real_t>(path[j].x), static_cast<real_t>(path[j].y)));
+		}
+		polypaths.push_back(polypath2);
+	}
+	return polypaths;
+}
+
+Vector<Vector3i> Geometry2D::partial_pack_rects(const Vector<Vector2i> &p_sizes, const Size2i &p_atlas_size) {
+	Vector<stbrp_node> nodes;
+	nodes.resize(p_atlas_size.width);
+	memset(nodes.ptrw(), 0, sizeof(stbrp_node) * nodes.size());
+
+	stbrp_context context;
+	stbrp_init_target(&context, p_atlas_size.width, p_atlas_size.height, nodes.ptrw(), p_atlas_size.width);
+
+	Vector<stbrp_rect> rects;
+	rects.resize(p_sizes.size());
+
+	for (int i = 0; i < p_sizes.size(); i++) {
+		rects.write[i].id = i;
+		rects.write[i].w = p_sizes[i].width;
+		rects.write[i].h = p_sizes[i].height;
+		rects.write[i].x = 0;
+		rects.write[i].y = 0;
+		rects.write[i].was_packed = 0;
+	}
+
+	stbrp_pack_rects(&context, rects.ptrw(), rects.size());
+
+	Vector<Vector3i> ret;
+	ret.resize(p_sizes.size());
+
+	for (int i = 0; i < p_sizes.size(); i++) {
+		ret.write[rects[i].id] = Vector3i(rects[i].x, rects[i].y, rects[i].was_packed != 0 ? 1 : 0);
+	}
+
+	return ret;
+}

core/math/geometry_2d.h

@@ -0,0 +1,513 @@
+/**************************************************************************/
+/*  geometry_2d.h                                                         */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "core/math/delaunay_2d.h"
+#include "core/math/math_funcs.h"
+#include "core/math/triangulate.h"
+#include "core/math/vector2.h"
+#include "core/math/vector2i.h"
+#include "core/math/vector3.h"
+#include "core/math/vector3i.h"
+#include "core/templates/vector.h"
+
+class Geometry2D {
+public:
+	static real_t get_closest_points_between_segments(const Vector2 &p1, const Vector2 &q1, const Vector2 &p2, const Vector2 &q2, Vector2 &c1, Vector2 &c2) {
+		Vector2 d1 = q1 - p1; // Direction vector of segment S1.
+		Vector2 d2 = q2 - p2; // Direction vector of segment S2.
+		Vector2 r = p1 - p2;
+		real_t a = d1.dot(d1); // Squared length of segment S1, always nonnegative.
+		real_t e = d2.dot(d2); // Squared length of segment S2, always nonnegative.
+		real_t f = d2.dot(r);
+		real_t s, t;
+		// Check if either or both segments degenerate into points.
+		if (a <= (real_t)CMP_EPSILON && e <= (real_t)CMP_EPSILON) {
+			// Both segments degenerate into points.
+			c1 = p1;
+			c2 = p2;
+			return Math::sqrt((c1 - c2).dot(c1 - c2));
+		}
+		if (a <= (real_t)CMP_EPSILON) {
+			// First segment degenerates into a point.
+			s = 0.0;
+			t = f / e; // s = 0 => t = (b*s + f) / e = f / e
+			t = CLAMP(t, 0.0f, 1.0f);
+		} else {
+			real_t c = d1.dot(r);
+			if (e <= (real_t)CMP_EPSILON) {
+				// Second segment degenerates into a point.
+				t = 0.0;
+				s = CLAMP(-c / a, 0.0f, 1.0f); // t = 0 => s = (b*t - c) / a = -c / a
+			} else {
+				// The general nondegenerate case starts here.
+				real_t b = d1.dot(d2);
+				real_t denom = a * e - b * b; // Always nonnegative.
+				// If segments not parallel, compute closest point on L1 to L2 and
+				// clamp to segment S1. Else pick arbitrary s (here 0).
+				if (denom != 0.0f) {
+					s = CLAMP((b * f - c * e) / denom, 0.0f, 1.0f);
+				} else {
+					s = 0.0;
+				}
+				// Compute point on L2 closest to S1(s) using
+				// t = Dot((P1 + D1*s) - P2,D2) / Dot(D2,D2) = (b*s + f) / e
+				t = (b * s + f) / e;
+
+				//If t in [0,1] done. Else clamp t, recompute s for the new value
+				// of t using s = Dot((P2 + D2*t) - P1,D1) / Dot(D1,D1)= (t*b - c) / a
+				// and clamp s to [0, 1].
+				if (t < 0.0f) {
+					t = 0.0;
+					s = CLAMP(-c / a, 0.0f, 1.0f);
+				} else if (t > 1.0f) {
+					t = 1.0;
+					s = CLAMP((b - c) / a, 0.0f, 1.0f);
+				}
+			}
+		}
+		c1 = p1 + d1 * s;
+		c2 = p2 + d2 * t;
+		return Math::sqrt((c1 - c2).dot(c1 - c2));
+	}
+
+#ifndef DISABLE_DEPRECATED
+	static Vector2 get_closest_point_to_segment(const Vector2 &p_point, const Vector2 *p_segment) {
+		return get_closest_point_to_segment(p_point, p_segment[0], p_segment[1]);
+	}
+#endif // DISABLE_DEPRECATED
+
+	static Vector2 get_closest_point_to_segment(const Vector2 &p_point, const Vector2 &p_segment_a, const Vector2 &p_segment_b) {
+		Vector2 p = p_point - p_segment_a;
+		Vector2 n = p_segment_b - p_segment_a;
+		real_t l2 = n.length_squared();
+		if (l2 < 1e-20f) {
+			return p_segment_a; // Both points are the same, just give any.
+		}
+
+		real_t d = n.dot(p) / l2;
+
+		if (d <= 0.0f) {
+			return p_segment_a; // Before first point.
+		} else if (d >= 1.0f) {
+			return p_segment_b; // After first point.
+		} else {
+			return p_segment_a + n * d; // Inside.
+		}
+	}
+
+#ifndef DISABLE_DEPRECATED
+	static real_t get_distance_to_segment(const Vector2 &p_point, const Vector2 *p_segment) {
+		return get_distance_to_segment(p_point, p_segment[0], p_segment[1]);
+	}
+#endif // DISABLE_DEPRECATED
+
+	static real_t get_distance_to_segment(const Vector2 &p_point, const Vector2 &p_segment_a, const Vector2 &p_segment_b) {
+		return p_point.distance_to(get_closest_point_to_segment(p_point, p_segment_a, p_segment_b));
+	}
+
+	static bool is_point_in_triangle(const Vector2 &s, const Vector2 &a, const Vector2 &b, const Vector2 &c) {
+		Vector2 an = a - s;
+		Vector2 bn = b - s;
+		Vector2 cn = c - s;
+
+		bool orientation = an.cross(bn) > 0;
+
+		if ((bn.cross(cn) > 0) != orientation) {
+			return false;
+		}
+
+		return (cn.cross(an) > 0) == orientation;
+	}
+
+#ifndef DISABLE_DEPRECATED
+	static Vector2 get_closest_point_to_segment_uncapped(const Vector2 &p_point, const Vector2 *p_segment) {
+		return get_closest_point_to_segment_uncapped(p_point, p_segment[0], p_segment[1]);
+	}
+#endif // DISABLE_DEPRECATED
+
+	static Vector2 get_closest_point_to_segment_uncapped(const Vector2 &p_point, const Vector2 &p_segment_a, const Vector2 &p_segment_b) {
+		Vector2 p = p_point - p_segment_a;
+		Vector2 n = p_segment_b - p_segment_a;
+		real_t l2 = n.length_squared();
+		if (l2 < 1e-20f) {
+			return p_segment_a; // Both points are the same, just give any.
+		}
+
+		real_t d = n.dot(p) / l2;
+
+		return p_segment_a + n * d; // Inside.
+	}
+
+	GODOT_MSVC_WARNING_PUSH_AND_IGNORE(4723) // Potential divide by 0. False positive (see: GH-44274).
+
+	static bool line_intersects_line(const Vector2 &p_from_a, const Vector2 &p_dir_a, const Vector2 &p_from_b, const Vector2 &p_dir_b, Vector2 &r_result) {
+		// See http://paulbourke.net/geometry/pointlineplane/
+
+		const real_t denom = p_dir_b.y * p_dir_a.x - p_dir_b.x * p_dir_a.y;
+		if (Math::is_zero_approx(denom)) { // Parallel?
+			return false;
+		}
+
+		const Vector2 v = p_from_a - p_from_b;
+		const real_t t = (p_dir_b.x * v.y - p_dir_b.y * v.x) / denom;
+		r_result = p_from_a + t * p_dir_a;
+		return true;
+	}
+
+	GODOT_MSVC_WARNING_POP
+
+	static bool segment_intersects_segment(const Vector2 &p_from_a, const Vector2 &p_to_a, const Vector2 &p_from_b, const Vector2 &p_to_b, Vector2 *r_result) {
+		Vector2 B = p_to_a - p_from_a;
+		Vector2 C = p_from_b - p_from_a;
+		Vector2 D = p_to_b - p_from_a;
+
+		real_t ABlen = B.dot(B);
+		if (ABlen <= 0) {
+			return false;
+		}
+		Vector2 Bn = B / ABlen;
+		C = Vector2(C.x * Bn.x + C.y * Bn.y, C.y * Bn.x - C.x * Bn.y);
+		D = Vector2(D.x * Bn.x + D.y * Bn.y, D.y * Bn.x - D.x * Bn.y);
+
+		// Fail if C x B and D x B have the same sign (segments don't intersect).
+		if ((C.y < (real_t)-CMP_EPSILON && D.y < (real_t)-CMP_EPSILON) || (C.y > (real_t)CMP_EPSILON && D.y > (real_t)CMP_EPSILON)) {
+			return false;
+		}
+
+		// Fail if segments are parallel or colinear.
+		// (when A x B == zero, i.e (C - D) x B == zero, i.e C x B == D x B)
+		if (Math::is_equal_approx(C.y, D.y)) {
+			return false;
+		}
+
+		real_t ABpos = D.x + (C.x - D.x) * D.y / (D.y - C.y);
+
+		// Fail if segment C-D crosses line A-B outside of segment A-B.
+		if ((ABpos < 0) || (ABpos > 1)) {
+			return false;
+		}
+
+		// Apply the discovered position to line A-B in the original coordinate system.
+		if (r_result) {
+			*r_result = p_from_a + B * ABpos;
+		}
+
+		return true;
+	}
+
+	static inline bool is_point_in_circle(const Vector2 &p_point, const Vector2 &p_circle_pos, real_t p_circle_radius) {
+		return p_point.distance_squared_to(p_circle_pos) <= p_circle_radius * p_circle_radius;
+	}
+
+	static real_t segment_intersects_circle(const Vector2 &p_from, const Vector2 &p_to, const Vector2 &p_circle_pos, real_t p_circle_radius) {
+		Vector2 line_vec = p_to - p_from;
+		Vector2 vec_to_line = p_from - p_circle_pos;
+
+		// Create a quadratic formula of the form ax^2 + bx + c = 0
+		real_t a, b, c;
+
+		a = line_vec.dot(line_vec);
+		b = 2 * vec_to_line.dot(line_vec);
+		c = vec_to_line.dot(vec_to_line) - p_circle_radius * p_circle_radius;
+
+		// Solve for t.
+		real_t sqrtterm = b * b - 4 * a * c;
+
+		// If the term we intend to square root is less than 0 then the answer won't be real,
+		// so it definitely won't be t in the range 0 to 1.
+		if (sqrtterm < 0) {
+			return -1;
+		}
+
+		// If we can assume that the line segment starts outside the circle (e.g. for continuous time collision detection)
+		// then the following can be skipped and we can just return the equivalent of res1.
+		sqrtterm = Math::sqrt(sqrtterm);
+		real_t res1 = (-b - sqrtterm) / (2 * a);
+		real_t res2 = (-b + sqrtterm) / (2 * a);
+
+		if (res1 >= 0 && res1 <= 1) {
+			return res1;
+		}
+		if (res2 >= 0 && res2 <= 1) {
+			return res2;
+		}
+		return -1;
+	}
+
+	static bool segment_intersects_rect(const Vector2 &p_from, const Vector2 &p_to, const Rect2 &p_rect) {
+		if (p_rect.has_point(p_from) || p_rect.has_point(p_to)) {
+			return true;
+		}
+
+		const Vector2 rect_points[4] = {
+			p_rect.position,
+			p_rect.position + Vector2(p_rect.size.x, 0),
+			p_rect.position + p_rect.size,
+			p_rect.position + Vector2(0, p_rect.size.y)
+		};
+
+		// Check if any of the rect's edges intersect the segment.
+		for (int i = 0; i < 4; i++) {
+			if (segment_intersects_segment(p_from, p_to, rect_points[i], rect_points[(i + 1) % 4], nullptr)) {
+				return true;
+			}
+		}
+
+		return false;
+	}
+
+	enum PolyBooleanOperation {
+		OPERATION_UNION,
+		OPERATION_DIFFERENCE,
+		OPERATION_INTERSECTION,
+		OPERATION_XOR
+	};
+	enum PolyJoinType {
+		JOIN_SQUARE,
+		JOIN_ROUND,
+		JOIN_MITER
+	};
+	enum PolyEndType {
+		END_POLYGON,
+		END_JOINED,
+		END_BUTT,
+		END_SQUARE,
+		END_ROUND
+	};
+
+	static Vector<Vector<Point2>> merge_polygons(const Vector<Point2> &p_polygon_a, const Vector<Point2> &p_polygon_b) {
+		return _polypaths_do_operation(OPERATION_UNION, p_polygon_a, p_polygon_b);
+	}
+
+	static Vector<Vector<Point2>> clip_polygons(const Vector<Point2> &p_polygon_a, const Vector<Point2> &p_polygon_b) {
+		return _polypaths_do_operation(OPERATION_DIFFERENCE, p_polygon_a, p_polygon_b);
+	}
+
+	static Vector<Vector<Point2>> intersect_polygons(const Vector<Point2> &p_polygon_a, const Vector<Point2> &p_polygon_b) {
+		return _polypaths_do_operation(OPERATION_INTERSECTION, p_polygon_a, p_polygon_b);
+	}
+
+	static Vector<Vector<Point2>> exclude_polygons(const Vector<Point2> &p_polygon_a, const Vector<Point2> &p_polygon_b) {
+		return _polypaths_do_operation(OPERATION_XOR, p_polygon_a, p_polygon_b);
+	}
+
+	static Vector<Vector<Point2>> clip_polyline_with_polygon(const Vector<Vector2> &p_polyline, const Vector<Vector2> &p_polygon) {
+		return _polypaths_do_operation(OPERATION_DIFFERENCE, p_polyline, p_polygon, true);
+	}
+
+	static Vector<Vector<Point2>> intersect_polyline_with_polygon(const Vector<Vector2> &p_polyline, const Vector<Vector2> &p_polygon) {
+		return _polypaths_do_operation(OPERATION_INTERSECTION, p_polyline, p_polygon, true);
+	}
+
+	static Vector<Vector<Point2>> offset_polygon(const Vector<Vector2> &p_polygon, real_t p_delta, PolyJoinType p_join_type) {
+		return _polypath_offset(p_polygon, p_delta, p_join_type, END_POLYGON);
+	}
+
+	static Vector<Vector<Point2>> offset_polyline(const Vector<Vector2> &p_polygon, real_t p_delta, PolyJoinType p_join_type, PolyEndType p_end_type) {
+		ERR_FAIL_COND_V_MSG(p_end_type == END_POLYGON, Vector<Vector<Point2>>(), "Attempt to offset a polyline like a polygon (use offset_polygon instead).");
+
+		return _polypath_offset(p_polygon, p_delta, p_join_type, p_end_type);
+	}
+
+	static Vector<int> triangulate_delaunay(const Vector<Vector2> &p_points) {
+		Vector<Delaunay2D::Triangle> tr = Delaunay2D::triangulate(p_points);
+		Vector<int> triangles;
+
+		triangles.resize(3 * tr.size());
+		int *ptr = triangles.ptrw();
+		for (int i = 0; i < tr.size(); i++) {
+			*ptr++ = tr[i].points[0];
+			*ptr++ = tr[i].points[1];
+			*ptr++ = tr[i].points[2];
+		}
+		return triangles;
+	}
+
+	static Vector<int> triangulate_polygon(const Vector<Vector2> &p_polygon) {
+		Vector<int> triangles;
+		if (!Triangulate::triangulate(p_polygon, triangles)) {
+			return Vector<int>(); //fail
+		}
+		return triangles;
+	}
+
+	// Assumes cartesian coordinate system with +x to the right, +y up.
+	// If using screen coordinates (+x to the right, +y down) the result will need to be flipped.
+	static bool is_polygon_clockwise(const Vector<Vector2> &p_polygon) {
+		int c = p_polygon.size();
+		if (c < 3) {
+			return false;
+		}
+		const Vector2 *p = p_polygon.ptr();
+		real_t sum = 0;
+		for (int i = 0; i < c; i++) {
+			const Vector2 &v1 = p[i];
+			const Vector2 &v2 = p[(i + 1) % c];
+			sum += (v2.x - v1.x) * (v2.y + v1.y);
+		}
+
+		return sum > 0.0f;
+	}
+
+	// Alternate implementation that should be faster.
+	static bool is_point_in_polygon(const Vector2 &p_point, const Vector<Vector2> &p_polygon) {
+		int c = p_polygon.size();
+		if (c < 3) {
+			return false;
+		}
+		const Vector2 *p = p_polygon.ptr();
+		Vector2 further_away(-1e20, -1e20);
+		Vector2 further_away_opposite(1e20, 1e20);
+
+		for (int i = 0; i < c; i++) {
+			further_away = further_away.max(p[i]);
+			further_away_opposite = further_away_opposite.min(p[i]);
+		}
+
+		// Make point outside that won't intersect with points in segment from p_point.
+		further_away += (further_away - further_away_opposite) * Vector2(1.221313, 1.512312);
+
+		int intersections = 0;
+		for (int i = 0; i < c; i++) {
+			const Vector2 &v1 = p[i];
+			const Vector2 &v2 = p[(i + 1) % c];
+
+			Vector2 res;
+			if (segment_intersects_segment(v1, v2, p_point, further_away, &res)) {
+				intersections++;
+				if (res.is_equal_approx(p_point)) {
+					// Point is in one of the polygon edges.
+					return true;
+				}
+			}
+		}
+
+		return (intersections & 1);
+	}
+
+	static bool is_segment_intersecting_polygon(const Vector2 &p_from, const Vector2 &p_to, const Vector<Vector2> &p_polygon) {
+		int c = p_polygon.size();
+		const Vector2 *p = p_polygon.ptr();
+		for (int i = 0; i < c; i++) {
+			const Vector2 &v1 = p[i];
+			const Vector2 &v2 = p[(i + 1) % c];
+			if (segment_intersects_segment(p_from, p_to, v1, v2, nullptr)) {
+				return true;
+			}
+		}
+		return false;
+	}
+
+	static real_t vec2_cross(const Point2 &O, const Point2 &A, const Point2 &B) {
+		return (real_t)(A.x - O.x) * (B.y - O.y) - (real_t)(A.y - O.y) * (B.x - O.x);
+	}
+
+	// Returns a list of points on the convex hull in counter-clockwise order.
+	// Note: the last point in the returned list is the same as the first one.
+	static Vector<Point2> convex_hull(Vector<Point2> P) {
+		int n = P.size(), k = 0;
+		Vector<Point2> H;
+		H.resize(2 * n);
+
+		// Sort points lexicographically.
+		P.sort();
+
+		// Build lower hull.
+		for (int i = 0; i < n; ++i) {
+			while (k >= 2 && vec2_cross(H[k - 2], H[k - 1], P[i]) <= 0) {
+				k--;
+			}
+			H.write[k++] = P[i];
+		}
+
+		// Build upper hull.
+		for (int i = n - 2, t = k + 1; i >= 0; i--) {
+			while (k >= t && vec2_cross(H[k - 2], H[k - 1], P[i]) <= 0) {
+				k--;
+			}
+			H.write[k++] = P[i];
+		}
+
+		H.resize(k);
+		return H;
+	}
+
+	static Vector<Point2i> bresenham_line(const Point2i &p_from, const Point2i &p_to) {
+		Vector<Point2i> points;
+
+		Vector2i delta = (p_to - p_from).abs() * 2;
+		Vector2i step = (p_to - p_from).sign();
+		Vector2i current = p_from;
+
+		if (delta.x > delta.y) {
+			int err = delta.x / 2;
+
+			for (; current.x != p_to.x; current.x += step.x) {
+				points.push_back(current);
+
+				err -= delta.y;
+				if (err < 0) {
+					current.y += step.y;
+					err += delta.x;
+				}
+			}
+		} else {
+			int err = delta.y / 2;
+
+			for (; current.y != p_to.y; current.y += step.y) {
+				points.push_back(current);
+
+				err -= delta.x;
+				if (err < 0) {
+					current.x += step.x;
+					err += delta.y;
+				}
+			}
+		}
+
+		points.push_back(current);
+
+		return points;
+	}
+
+	static void merge_many_polygons(const Vector<Vector<Point2>> &p_polygons, Vector<Vector<Vector2>> &r_out_polygons, Vector<Vector<Vector2>> &r_out_holes);
+	static Vector<Vector<Vector2>> decompose_many_polygons_in_convex(const Vector<Vector<Point2>> &p_polygons, const Vector<Vector<Point2>> &p_holes);
+
+	static Vector<Vector<Vector2>> decompose_polygon_in_convex(const Vector<Point2> &p_polygon);
+
+	static void make_atlas(const Vector<Size2i> &p_rects, Vector<Point2i> &r_result, Size2i &r_size);
+	static Vector<Vector3i> partial_pack_rects(const Vector<Vector2i> &p_sizes, const Size2i &p_atlas_size);
+
+private:
+	static Vector<Vector<Point2>> _polypaths_do_operation(PolyBooleanOperation p_op, const Vector<Point2> &p_polypath_a, const Vector<Point2> &p_polypath_b, bool is_a_open = false);
+	static Vector<Vector<Point2>> _polypath_offset(const Vector<Point2> &p_polypath, real_t p_delta, PolyJoinType p_join_type, PolyEndType p_end_type);
+};

core/math/geometry_3d.cpp

@@ -0,0 +1,987 @@
+/**************************************************************************/
+/*  geometry_3d.cpp                                                       */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "geometry_3d.h"
+
+#include "core/templates/hash_map.h"
+
+void Geometry3D::get_closest_points_between_segments(const Vector3 &p_p0, const Vector3 &p_p1, const Vector3 &p_q0, const Vector3 &p_q1, Vector3 &r_ps, Vector3 &r_qt) {
+	// Based on David Eberly's Computation of Distance Between Line Segments algorithm.
+
+	Vector3 p = p_p1 - p_p0;
+	Vector3 q = p_q1 - p_q0;
+	Vector3 r = p_p0 - p_q0;
+
+	real_t a = p.dot(p);
+	real_t b = p.dot(q);
+	real_t c = q.dot(q);
+	real_t d = p.dot(r);
+	real_t e = q.dot(r);
+
+	real_t s = 0.0f;
+	real_t t = 0.0f;
+
+	real_t det = a * c - b * b;
+	if (det > CMP_EPSILON) {
+		// Non-parallel segments
+		real_t bte = b * e;
+		real_t ctd = c * d;
+
+		if (bte <= ctd) {
+			// s <= 0.0f
+			if (e <= 0.0f) {
+				// t <= 0.0f
+				s = (-d >= a ? 1 : (-d > 0.0f ? -d / a : 0.0f));
+				t = 0.0f;
+			} else if (e < c) {
+				// 0.0f < t < 1
+				s = 0.0f;
+				t = e / c;
+			} else {
+				// t >= 1
+				s = (b - d >= a ? 1 : (b - d > 0.0f ? (b - d) / a : 0.0f));
+				t = 1;
+			}
+		} else {
+			// s > 0.0f
+			s = bte - ctd;
+			if (s >= det) {
+				// s >= 1
+				if (b + e <= 0.0f) {
+					// t <= 0.0f
+					s = (-d <= 0.0f ? 0.0f : (-d < a ? -d / a : 1));
+					t = 0.0f;
+				} else if (b + e < c) {
+					// 0.0f < t < 1
+					s = 1;
+					t = (b + e) / c;
+				} else {
+					// t >= 1
+					s = (b - d <= 0.0f ? 0.0f : (b - d < a ? (b - d) / a : 1));
+					t = 1;
+				}
+			} else {
+				// 0.0f < s < 1
+				real_t ate = a * e;
+				real_t btd = b * d;
+
+				if (ate <= btd) {
+					// t <= 0.0f
+					s = (-d <= 0.0f ? 0.0f : (-d >= a ? 1 : -d / a));
+					t = 0.0f;
+				} else {
+					// t > 0.0f
+					t = ate - btd;
+					if (t >= det) {
+						// t >= 1
+						s = (b - d <= 0.0f ? 0.0f : (b - d >= a ? 1 : (b - d) / a));
+						t = 1;
+					} else {
+						// 0.0f < t < 1
+						s /= det;
+						t /= det;
+					}
+				}
+			}
+		}
+	} else {
+		// Parallel segments
+		if (e <= 0.0f) {
+			s = (-d <= 0.0f ? 0.0f : (-d >= a ? 1 : -d / a));
+			t = 0.0f;
+		} else if (e >= c) {
+			s = (b - d <= 0.0f ? 0.0f : (b - d >= a ? 1 : (b - d) / a));
+			t = 1;
+		} else {
+			s = 0.0f;
+			t = e / c;
+		}
+	}
+
+	r_ps = (1 - s) * p_p0 + s * p_p1;
+	r_qt = (1 - t) * p_q0 + t * p_q1;
+}
+
+real_t Geometry3D::get_closest_distance_between_segments(const Vector3 &p_p0, const Vector3 &p_p1, const Vector3 &p_q0, const Vector3 &p_q1) {
+	Vector3 ps;
+	Vector3 qt;
+	get_closest_points_between_segments(p_p0, p_p1, p_q0, p_q1, ps, qt);
+	Vector3 st = qt - ps;
+	return st.length();
+}
+
+void Geometry3D::MeshData::optimize_vertices() {
+	HashMap<int, int> vtx_remap;
+
+	for (MeshData::Face &face : faces) {
+		for (int &index : face.indices) {
+			if (!vtx_remap.has(index)) {
+				int ni = vtx_remap.size();
+				vtx_remap[index] = ni;
+			}
+			index = vtx_remap[index];
+		}
+	}
+
+	for (MeshData::Edge &edge : edges) {
+		int a = edge.vertex_a;
+		int b = edge.vertex_b;
+
+		if (!vtx_remap.has(a)) {
+			int ni = vtx_remap.size();
+			vtx_remap[a] = ni;
+		}
+		if (!vtx_remap.has(b)) {
+			int ni = vtx_remap.size();
+			vtx_remap[b] = ni;
+		}
+
+		edge.vertex_a = vtx_remap[a];
+		edge.vertex_b = vtx_remap[b];
+	}
+
+	LocalVector<Vector3> new_vertices;
+	new_vertices.resize(vtx_remap.size());
+
+	for (uint32_t i = 0; i < vertices.size(); i++) {
+		if (vtx_remap.has(i)) {
+			new_vertices[vtx_remap[i]] = vertices[i];
+		}
+	}
+	vertices = new_vertices;
+}
+
+struct _FaceClassify {
+	struct _Link {
+		int face = -1;
+		int edge = -1;
+		void clear() {
+			face = -1;
+			edge = -1;
+		}
+		_Link() {}
+	};
+	bool valid = false;
+	int group = -1;
+	_Link links[3];
+	Face3 face;
+	_FaceClassify() {}
+};
+
+/*** GEOMETRY WRAPPER ***/
+
+enum _CellFlags {
+	_CELL_SOLID = 1,
+	_CELL_EXTERIOR = 2,
+	_CELL_STEP_MASK = 0x1C,
+	_CELL_STEP_NONE = 0 << 2,
+	_CELL_STEP_Y_POS = 1 << 2,
+	_CELL_STEP_Y_NEG = 2 << 2,
+	_CELL_STEP_X_POS = 3 << 2,
+	_CELL_STEP_X_NEG = 4 << 2,
+	_CELL_STEP_Z_POS = 5 << 2,
+	_CELL_STEP_Z_NEG = 6 << 2,
+	_CELL_STEP_DONE = 7 << 2,
+	_CELL_PREV_MASK = 0xE0,
+	_CELL_PREV_NONE = 0 << 5,
+	_CELL_PREV_Y_POS = 1 << 5,
+	_CELL_PREV_Y_NEG = 2 << 5,
+	_CELL_PREV_X_POS = 3 << 5,
+	_CELL_PREV_X_NEG = 4 << 5,
+	_CELL_PREV_Z_POS = 5 << 5,
+	_CELL_PREV_Z_NEG = 6 << 5,
+	_CELL_PREV_FIRST = 7 << 5,
+};
+
+static inline void _plot_face(uint8_t ***p_cell_status, int x, int y, int z, int len_x, int len_y, int len_z, const Vector3 &voxelsize, const Face3 &p_face) {
+	AABB aabb(Vector3(x, y, z), Vector3(len_x, len_y, len_z));
+	aabb.position = aabb.position * voxelsize;
+	aabb.size = aabb.size * voxelsize;
+
+	if (!p_face.intersects_aabb(aabb)) {
+		return;
+	}
+
+	if (len_x == 1 && len_y == 1 && len_z == 1) {
+		p_cell_status[x][y][z] = _CELL_SOLID;
+		return;
+	}
+
+	int div_x = len_x > 1 ? 2 : 1;
+	int div_y = len_y > 1 ? 2 : 1;
+	int div_z = len_z > 1 ? 2 : 1;
+
+#define SPLIT_DIV(m_i, m_div, m_v, m_len_v, m_new_v, m_new_len_v) \
+	if (m_div == 1) {                                             \
+		m_new_v = m_v;                                            \
+		m_new_len_v = 1;                                          \
+	} else if (m_i == 0) {                                        \
+		m_new_v = m_v;                                            \
+		m_new_len_v = m_len_v / 2;                                \
+	} else {                                                      \
+		m_new_v = m_v + m_len_v / 2;                              \
+		m_new_len_v = m_len_v - m_len_v / 2;                      \
+	}
+
+	int new_x;
+	int new_len_x;
+	int new_y;
+	int new_len_y;
+	int new_z;
+	int new_len_z;
+
+	for (int i = 0; i < div_x; i++) {
+		SPLIT_DIV(i, div_x, x, len_x, new_x, new_len_x);
+
+		for (int j = 0; j < div_y; j++) {
+			SPLIT_DIV(j, div_y, y, len_y, new_y, new_len_y);
+
+			for (int k = 0; k < div_z; k++) {
+				SPLIT_DIV(k, div_z, z, len_z, new_z, new_len_z);
+
+				_plot_face(p_cell_status, new_x, new_y, new_z, new_len_x, new_len_y, new_len_z, voxelsize, p_face);
+			}
+		}
+	}
+
+#undef SPLIT_DIV
+}
+
+static inline void _mark_outside(uint8_t ***p_cell_status, int x, int y, int z, int len_x, int len_y, int len_z) {
+	if (p_cell_status[x][y][z] & 3) {
+		return; // Nothing to do, already used and/or visited.
+	}
+
+	p_cell_status[x][y][z] = _CELL_PREV_FIRST;
+
+	while (true) {
+		uint8_t &c = p_cell_status[x][y][z];
+
+		if ((c & _CELL_STEP_MASK) == _CELL_STEP_NONE) {
+			// Haven't been in here, mark as outside.
+			p_cell_status[x][y][z] |= _CELL_EXTERIOR;
+		}
+
+		if ((c & _CELL_STEP_MASK) != _CELL_STEP_DONE) {
+			// If not done, increase step.
+			c += 1 << 2;
+		}
+
+		if ((c & _CELL_STEP_MASK) == _CELL_STEP_DONE) {
+			// Go back.
+			switch (c & _CELL_PREV_MASK) {
+				case _CELL_PREV_FIRST: {
+					return;
+				} break;
+				case _CELL_PREV_Y_POS: {
+					y++;
+					ERR_FAIL_COND(y >= len_y);
+				} break;
+				case _CELL_PREV_Y_NEG: {
+					y--;
+					ERR_FAIL_COND(y < 0);
+				} break;
+				case _CELL_PREV_X_POS: {
+					x++;
+					ERR_FAIL_COND(x >= len_x);
+				} break;
+				case _CELL_PREV_X_NEG: {
+					x--;
+					ERR_FAIL_COND(x < 0);
+				} break;
+				case _CELL_PREV_Z_POS: {
+					z++;
+					ERR_FAIL_COND(z >= len_z);
+				} break;
+				case _CELL_PREV_Z_NEG: {
+					z--;
+					ERR_FAIL_COND(z < 0);
+				} break;
+				default: {
+					ERR_FAIL();
+				}
+			}
+			continue;
+		}
+
+		int next_x = x, next_y = y, next_z = z;
+		uint8_t prev = 0;
+
+		switch (c & _CELL_STEP_MASK) {
+			case _CELL_STEP_Y_POS: {
+				next_y++;
+				prev = _CELL_PREV_Y_NEG;
+			} break;
+			case _CELL_STEP_Y_NEG: {
+				next_y--;
+				prev = _CELL_PREV_Y_POS;
+			} break;
+			case _CELL_STEP_X_POS: {
+				next_x++;
+				prev = _CELL_PREV_X_NEG;
+			} break;
+			case _CELL_STEP_X_NEG: {
+				next_x--;
+				prev = _CELL_PREV_X_POS;
+			} break;
+			case _CELL_STEP_Z_POS: {
+				next_z++;
+				prev = _CELL_PREV_Z_NEG;
+			} break;
+			case _CELL_STEP_Z_NEG: {
+				next_z--;
+				prev = _CELL_PREV_Z_POS;
+			} break;
+			default:
+				ERR_FAIL();
+		}
+
+		if (next_x < 0 || next_x >= len_x) {
+			continue;
+		}
+		if (next_y < 0 || next_y >= len_y) {
+			continue;
+		}
+		if (next_z < 0 || next_z >= len_z) {
+			continue;
+		}
+
+		if (p_cell_status[next_x][next_y][next_z] & 3) {
+			continue;
+		}
+
+		x = next_x;
+		y = next_y;
+		z = next_z;
+		p_cell_status[x][y][z] |= prev;
+	}
+}
+
+static inline void _build_faces(uint8_t ***p_cell_status, int x, int y, int z, int len_x, int len_y, int len_z, Vector<Face3> &p_faces) {
+	ERR_FAIL_INDEX(x, len_x);
+	ERR_FAIL_INDEX(y, len_y);
+	ERR_FAIL_INDEX(z, len_z);
+
+	if (p_cell_status[x][y][z] & _CELL_EXTERIOR) {
+		return;
+	}
+
+#define vert(m_idx) Vector3(((m_idx) & 4) >> 2, ((m_idx) & 2) >> 1, (m_idx) & 1)
+
+	static const uint8_t indices[6][4] = {
+		{ 7, 6, 4, 5 },
+		{ 7, 3, 2, 6 },
+		{ 7, 5, 1, 3 },
+		{ 0, 2, 3, 1 },
+		{ 0, 1, 5, 4 },
+		{ 0, 4, 6, 2 },
+
+	};
+
+	for (int i = 0; i < 6; i++) {
+		Vector3 face_points[4];
+		int disp_x = x + ((i % 3) == 0 ? ((i < 3) ? 1 : -1) : 0);
+		int disp_y = y + (((i - 1) % 3) == 0 ? ((i < 3) ? 1 : -1) : 0);
+		int disp_z = z + (((i - 2) % 3) == 0 ? ((i < 3) ? 1 : -1) : 0);
+
+		bool plot = false;
+
+		if (disp_x < 0 || disp_x >= len_x) {
+			plot = true;
+		}
+		if (disp_y < 0 || disp_y >= len_y) {
+			plot = true;
+		}
+		if (disp_z < 0 || disp_z >= len_z) {
+			plot = true;
+		}
+
+		if (!plot && (p_cell_status[disp_x][disp_y][disp_z] & _CELL_EXTERIOR)) {
+			plot = true;
+		}
+
+		if (!plot) {
+			continue;
+		}
+
+		for (int j = 0; j < 4; j++) {
+			face_points[j] = vert(indices[i][j]) + Vector3(x, y, z);
+		}
+
+		p_faces.push_back(
+				Face3(
+						face_points[0],
+						face_points[1],
+						face_points[2]));
+
+		p_faces.push_back(
+				Face3(
+						face_points[2],
+						face_points[3],
+						face_points[0]));
+	}
+}
+
+Vector<Face3> Geometry3D::wrap_geometry(const Vector<Face3> &p_array, real_t *p_error) {
+	int face_count = p_array.size();
+	const Face3 *faces = p_array.ptr();
+	constexpr double min_size = 1.0;
+	constexpr int max_length = 20;
+
+	AABB global_aabb;
+
+	for (int i = 0; i < face_count; i++) {
+		if (i == 0) {
+			global_aabb = faces[i].get_aabb();
+		} else {
+			global_aabb.merge_with(faces[i].get_aabb());
+		}
+	}
+
+	global_aabb.grow_by(0.01f); // Avoid numerical error.
+
+	// Determine amount of cells in grid axis.
+	int div_x, div_y, div_z;
+
+	if (global_aabb.size.x / min_size < max_length) {
+		div_x = (int)(global_aabb.size.x / min_size) + 1;
+	} else {
+		div_x = max_length;
+	}
+
+	if (global_aabb.size.y / min_size < max_length) {
+		div_y = (int)(global_aabb.size.y / min_size) + 1;
+	} else {
+		div_y = max_length;
+	}
+
+	if (global_aabb.size.z / min_size < max_length) {
+		div_z = (int)(global_aabb.size.z / min_size) + 1;
+	} else {
+		div_z = max_length;
+	}
+
+	Vector3 voxelsize = global_aabb.size;
+	voxelsize.x /= div_x;
+	voxelsize.y /= div_y;
+	voxelsize.z /= div_z;
+
+	// Create and initialize cells to zero.
+
+	uint8_t ***cell_status = memnew_arr(uint8_t **, div_x);
+	for (int i = 0; i < div_x; i++) {
+		cell_status[i] = memnew_arr(uint8_t *, div_y);
+
+		for (int j = 0; j < div_y; j++) {
+			cell_status[i][j] = memnew_arr(uint8_t, div_z);
+
+			for (int k = 0; k < div_z; k++) {
+				cell_status[i][j][k] = 0;
+			}
+		}
+	}
+
+	// Plot faces into cells.
+
+	for (int i = 0; i < face_count; i++) {
+		Face3 f = faces[i];
+		for (int j = 0; j < 3; j++) {
+			f.vertex[j] -= global_aabb.position;
+		}
+		_plot_face(cell_status, 0, 0, 0, div_x, div_y, div_z, voxelsize, f);
+	}
+
+	// Determine which cells connect to the outside by traversing the outside and recursively flood-fill marking.
+
+	for (int i = 0; i < div_x; i++) {
+		for (int j = 0; j < div_y; j++) {
+			_mark_outside(cell_status, i, j, 0, div_x, div_y, div_z);
+			_mark_outside(cell_status, i, j, div_z - 1, div_x, div_y, div_z);
+		}
+	}
+
+	for (int i = 0; i < div_z; i++) {
+		for (int j = 0; j < div_y; j++) {
+			_mark_outside(cell_status, 0, j, i, div_x, div_y, div_z);
+			_mark_outside(cell_status, div_x - 1, j, i, div_x, div_y, div_z);
+		}
+	}
+
+	for (int i = 0; i < div_x; i++) {
+		for (int j = 0; j < div_z; j++) {
+			_mark_outside(cell_status, i, 0, j, div_x, div_y, div_z);
+			_mark_outside(cell_status, i, div_y - 1, j, div_x, div_y, div_z);
+		}
+	}
+
+	// Build faces for the inside-outside cell divisors.
+
+	Vector<Face3> wrapped_faces;
+
+	for (int i = 0; i < div_x; i++) {
+		for (int j = 0; j < div_y; j++) {
+			for (int k = 0; k < div_z; k++) {
+				_build_faces(cell_status, i, j, k, div_x, div_y, div_z, wrapped_faces);
+			}
+		}
+	}
+
+	// Transform face vertices to global coords.
+
+	int wrapped_faces_count = wrapped_faces.size();
+	Face3 *wrapped_faces_ptr = wrapped_faces.ptrw();
+
+	for (int i = 0; i < wrapped_faces_count; i++) {
+		for (int j = 0; j < 3; j++) {
+			Vector3 &v = wrapped_faces_ptr[i].vertex[j];
+			v = v * voxelsize;
+			v += global_aabb.position;
+		}
+	}
+
+	// clean up grid
+
+	for (int i = 0; i < div_x; i++) {
+		for (int j = 0; j < div_y; j++) {
+			memdelete_arr(cell_status[i][j]);
+		}
+
+		memdelete_arr(cell_status[i]);
+	}
+
+	memdelete_arr(cell_status);
+	if (p_error) {
+		*p_error = voxelsize.length();
+	}
+
+	return wrapped_faces;
+}
+
+Geometry3D::MeshData Geometry3D::build_convex_mesh(const Vector<Plane> &p_planes) {
+	MeshData mesh;
+
+#define SUBPLANE_SIZE 1024.0
+
+	real_t subplane_size = 1024.0; // Should compute this from the actual plane.
+	for (int i = 0; i < p_planes.size(); i++) {
+		Plane p = p_planes[i];
+
+		Vector3 ref = Vector3(0.0, 1.0, 0.0);
+
+		if (Math::abs(p.normal.dot(ref)) > 0.95f) {
+			ref = Vector3(0.0, 0.0, 1.0); // Change axis.
+		}
+
+		Vector3 right = p.normal.cross(ref).normalized();
+		Vector3 up = p.normal.cross(right).normalized();
+
+		Vector3 center = p.get_center();
+
+		// make a quad clockwise
+		LocalVector<Vector3> vertices = {
+			center - up * subplane_size + right * subplane_size,
+			center - up * subplane_size - right * subplane_size,
+			center + up * subplane_size - right * subplane_size,
+			center + up * subplane_size + right * subplane_size
+		};
+
+		for (int j = 0; j < p_planes.size(); j++) {
+			if (j == i) {
+				continue;
+			}
+
+			LocalVector<Vector3> new_vertices;
+			Plane clip = p_planes[j];
+
+			if (clip.normal.dot(p.normal) > 0.95f) {
+				continue;
+			}
+
+			if (vertices.size() < 3) {
+				break;
+			}
+
+			for (uint32_t k = 0; k < vertices.size(); k++) {
+				int k_n = (k + 1) % vertices.size();
+
+				Vector3 edge0_A = vertices[k];
+				Vector3 edge1_A = vertices[k_n];
+
+				real_t dist0 = clip.distance_to(edge0_A);
+				real_t dist1 = clip.distance_to(edge1_A);
+
+				if (dist0 <= 0) { // Behind plane.
+
+					new_vertices.push_back(vertices[k]);
+				}
+
+				// Check for different sides and non coplanar.
+				if ((dist0 * dist1) < 0) {
+					// Calculate intersection.
+					Vector3 rel = edge1_A - edge0_A;
+
+					real_t den = clip.normal.dot(rel);
+					if (Math::is_zero_approx(den)) {
+						continue; // Point too short.
+					}
+
+					real_t dist = -(clip.normal.dot(edge0_A) - clip.d) / den;
+					Vector3 inters = edge0_A + rel * dist;
+					new_vertices.push_back(inters);
+				}
+			}
+
+			vertices = new_vertices;
+		}
+
+		if (vertices.size() < 3) {
+			continue;
+		}
+
+		// Result is a clockwise face.
+
+		MeshData::Face face;
+
+		// Add face indices.
+		for (const Vector3 &vertex : vertices) {
+			int idx = -1;
+			for (uint32_t k = 0; k < mesh.vertices.size(); k++) {
+				if (mesh.vertices[k].distance_to(vertex) < 0.001f) {
+					idx = k;
+					break;
+				}
+			}
+
+			if (idx == -1) {
+				idx = mesh.vertices.size();
+				mesh.vertices.push_back(vertex);
+			}
+
+			face.indices.push_back(idx);
+		}
+		face.plane = p;
+		mesh.faces.push_back(face);
+
+		// Add edge.
+
+		for (uint32_t j = 0; j < face.indices.size(); j++) {
+			int a = face.indices[j];
+			int b = face.indices[(j + 1) % face.indices.size()];
+
+			bool found = false;
+			int found_idx = -1;
+			for (uint32_t k = 0; k < mesh.edges.size(); k++) {
+				if (mesh.edges[k].vertex_a == a && mesh.edges[k].vertex_b == b) {
+					found = true;
+					found_idx = k;
+					break;
+				}
+				if (mesh.edges[k].vertex_b == a && mesh.edges[k].vertex_a == b) {
+					found = true;
+					found_idx = k;
+					break;
+				}
+			}
+
+			if (found) {
+				mesh.edges[found_idx].face_b = j;
+				continue;
+			}
+			MeshData::Edge edge;
+			edge.vertex_a = a;
+			edge.vertex_b = b;
+			edge.face_a = j;
+			edge.face_b = -1;
+			mesh.edges.push_back(edge);
+		}
+	}
+
+	return mesh;
+}
+
+Vector<Plane> Geometry3D::build_box_planes(const Vector3 &p_extents) {
+	Vector<Plane> planes = {
+		Plane(Vector3(1, 0, 0), p_extents.x),
+		Plane(Vector3(-1, 0, 0), p_extents.x),
+		Plane(Vector3(0, 1, 0), p_extents.y),
+		Plane(Vector3(0, -1, 0), p_extents.y),
+		Plane(Vector3(0, 0, 1), p_extents.z),
+		Plane(Vector3(0, 0, -1), p_extents.z)
+	};
+
+	return planes;
+}
+
+Vector<Plane> Geometry3D::build_cylinder_planes(real_t p_radius, real_t p_height, int p_sides, Vector3::Axis p_axis) {
+	ERR_FAIL_INDEX_V(p_axis, 3, Vector<Plane>());
+
+	Vector<Plane> planes;
+
+	const double sides_step = Math::TAU / p_sides;
+	for (int i = 0; i < p_sides; i++) {
+		Vector3 normal;
+		normal[(p_axis + 1) % 3] = Math::cos(i * sides_step);
+		normal[(p_axis + 2) % 3] = Math::sin(i * sides_step);
+
+		planes.push_back(Plane(normal, p_radius));
+	}
+
+	Vector3 axis;
+	axis[p_axis] = 1.0;
+
+	planes.push_back(Plane(axis, p_height * 0.5f));
+	planes.push_back(Plane(-axis, p_height * 0.5f));
+
+	return planes;
+}
+
+Vector<Plane> Geometry3D::build_sphere_planes(real_t p_radius, int p_lats, int p_lons, Vector3::Axis p_axis) {
+	ERR_FAIL_INDEX_V(p_axis, 3, Vector<Plane>());
+
+	Vector<Plane> planes;
+
+	Vector3 axis;
+	axis[p_axis] = 1.0;
+
+	Vector3 axis_neg;
+	axis_neg[(p_axis + 1) % 3] = 1.0;
+	axis_neg[(p_axis + 2) % 3] = 1.0;
+	axis_neg[p_axis] = -1.0;
+
+	const double lon_step = Math::TAU / p_lons;
+	for (int i = 0; i < p_lons; i++) {
+		Vector3 normal;
+		normal[(p_axis + 1) % 3] = Math::cos(i * lon_step);
+		normal[(p_axis + 2) % 3] = Math::sin(i * lon_step);
+
+		planes.push_back(Plane(normal, p_radius));
+
+		for (int j = 1; j <= p_lats; j++) {
+			Vector3 plane_normal = normal.lerp(axis, j / (real_t)p_lats).normalized();
+			planes.push_back(Plane(plane_normal, p_radius));
+			planes.push_back(Plane(plane_normal * axis_neg, p_radius));
+		}
+	}
+
+	return planes;
+}
+
+Vector<Plane> Geometry3D::build_capsule_planes(real_t p_radius, real_t p_height, int p_sides, int p_lats, Vector3::Axis p_axis) {
+	ERR_FAIL_INDEX_V(p_axis, 3, Vector<Plane>());
+
+	Vector<Plane> planes;
+
+	Vector3 axis;
+	axis[p_axis] = 1.0;
+
+	Vector3 axis_neg;
+	axis_neg[(p_axis + 1) % 3] = 1.0;
+	axis_neg[(p_axis + 2) % 3] = 1.0;
+	axis_neg[p_axis] = -1.0;
+
+	const double sides_step = Math::TAU / p_sides;
+	for (int i = 0; i < p_sides; i++) {
+		Vector3 normal;
+		normal[(p_axis + 1) % 3] = Math::cos(i * sides_step);
+		normal[(p_axis + 2) % 3] = Math::sin(i * sides_step);
+
+		planes.push_back(Plane(normal, p_radius));
+
+		for (int j = 1; j <= p_lats; j++) {
+			Vector3 plane_normal = normal.lerp(axis, j / (real_t)p_lats).normalized();
+			Vector3 position = axis * p_height * 0.5f + plane_normal * p_radius;
+			planes.push_back(Plane(plane_normal, position));
+			planes.push_back(Plane(plane_normal * axis_neg, position * axis_neg));
+		}
+	}
+
+	return planes;
+}
+
+Vector<Vector3> Geometry3D::compute_convex_mesh_points(const Plane *p_planes, int p_plane_count) {
+	Vector<Vector3> points;
+
+	// Iterate through every unique combination of any three planes.
+	for (int i = p_plane_count - 1; i >= 0; i--) {
+		for (int j = i - 1; j >= 0; j--) {
+			for (int k = j - 1; k >= 0; k--) {
+				// Find the point where these planes all cross over (if they
+				// do at all).
+				Vector3 convex_shape_point;
+				if (p_planes[i].intersect_3(p_planes[j], p_planes[k], &convex_shape_point)) {
+					// See if any *other* plane excludes this point because it's
+					// on the wrong side.
+					bool excluded = false;
+					for (int n = 0; n < p_plane_count; n++) {
+						if (n != i && n != j && n != k) {
+							real_t dp = p_planes[n].normal.dot(convex_shape_point);
+							if (dp - p_planes[n].d > (real_t)CMP_EPSILON) {
+								excluded = true;
+								break;
+							}
+						}
+					}
+
+					// Only add the point if it passed all tests.
+					if (!excluded) {
+						points.push_back(convex_shape_point);
+					}
+				}
+			}
+		}
+	}
+
+	return points;
+}
+
+#define square(m_s) ((m_s) * (m_s))
+#define BIG_VAL 1e20
+
+/* dt of 1d function using squared distance */
+static void edt(float *f, int stride, int n) {
+	float *d = (float *)alloca(sizeof(float) * n + sizeof(int) * n + sizeof(float) * (n + 1));
+	int *v = reinterpret_cast<int *>(&(d[n]));
+	float *z = reinterpret_cast<float *>(&v[n]);
+
+	int k = 0;
+	v[0] = 0;
+	z[0] = -BIG_VAL;
+	z[1] = +BIG_VAL;
+	for (int q = 1; q <= n - 1; q++) {
+		float s = ((f[q * stride] + square(q)) - (f[v[k] * stride] + square(v[k]))) / (2 * q - 2 * v[k]);
+		while (s <= z[k]) {
+			k--;
+			s = ((f[q * stride] + square(q)) - (f[v[k] * stride] + square(v[k]))) / (2 * q - 2 * v[k]);
+		}
+		k++;
+		v[k] = q;
+
+		z[k] = s;
+		z[k + 1] = +BIG_VAL;
+	}
+
+	k = 0;
+	for (int q = 0; q <= n - 1; q++) {
+		while (z[k + 1] < q) {
+			k++;
+		}
+		d[q] = square(q - v[k]) + f[v[k] * stride];
+	}
+
+	for (int i = 0; i < n; i++) {
+		f[i * stride] = d[i];
+	}
+}
+
+#undef square
+
+Vector<uint32_t> Geometry3D::generate_edf(const Vector<bool> &p_voxels, const Vector3i &p_size, bool p_negative) {
+	uint32_t float_count = p_size.x * p_size.y * p_size.z;
+
+	ERR_FAIL_COND_V((uint32_t)p_voxels.size() != float_count, Vector<uint32_t>());
+
+	float *work_memory = memnew_arr(float, float_count);
+	for (uint32_t i = 0; i < float_count; i++) {
+		work_memory[i] = BIG_VAL;
+	}
+
+	uint32_t y_mult = p_size.x;
+	uint32_t z_mult = y_mult * p_size.y;
+
+	//plot solid cells
+	{
+		const bool *voxr = p_voxels.ptr();
+		for (uint32_t i = 0; i < float_count; i++) {
+			bool plot = voxr[i];
+			if (p_negative) {
+				plot = !plot;
+			}
+			if (plot) {
+				work_memory[i] = 0;
+			}
+		}
+	}
+
+	//process in each direction
+
+	//xy->z
+
+	for (int i = 0; i < p_size.x; i++) {
+		for (int j = 0; j < p_size.y; j++) {
+			edt(&work_memory[i + j * y_mult], z_mult, p_size.z);
+		}
+	}
+
+	//xz->y
+
+	for (int i = 0; i < p_size.x; i++) {
+		for (int j = 0; j < p_size.z; j++) {
+			edt(&work_memory[i + j * z_mult], y_mult, p_size.y);
+		}
+	}
+
+	//yz->x
+	for (int i = 0; i < p_size.y; i++) {
+		for (int j = 0; j < p_size.z; j++) {
+			edt(&work_memory[i * y_mult + j * z_mult], 1, p_size.x);
+		}
+	}
+
+	Vector<uint32_t> ret;
+	ret.resize(float_count);
+	{
+		uint32_t *w = ret.ptrw();
+		for (uint32_t i = 0; i < float_count; i++) {
+			w[i] = uint32_t(Math::sqrt(work_memory[i]));
+		}
+	}
+
+	memdelete_arr(work_memory);
+
+	return ret;
+}
+
+#undef BIG_VAL
+
+Vector<int8_t> Geometry3D::generate_sdf8(const Vector<uint32_t> &p_positive, const Vector<uint32_t> &p_negative) {
+	ERR_FAIL_COND_V(p_positive.size() != p_negative.size(), Vector<int8_t>());
+	Vector<int8_t> sdf8;
+	int s = p_positive.size();
+	sdf8.resize(s);
+
+	const uint32_t *rpos = p_positive.ptr();
+	const uint32_t *rneg = p_negative.ptr();
+	int8_t *wsdf = sdf8.ptrw();
+	for (int i = 0; i < s; i++) {
+		int32_t diff = int32_t(rpos[i]) - int32_t(rneg[i]);
+		wsdf[i] = CLAMP(diff, -128, 127);
+	}
+	return sdf8;
+}

core/math/geometry_3d.h

@@ -0,0 +1,858 @@
+/**************************************************************************/
+/*  geometry_3d.h                                                         */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "core/math/delaunay_3d.h"
+#include "core/math/face3.h"
+#include "core/templates/local_vector.h"
+#include "core/templates/vector.h"
+
+class Geometry3D {
+public:
+	static void get_closest_points_between_segments(const Vector3 &p_p0, const Vector3 &p_p1, const Vector3 &p_q0, const Vector3 &p_q1, Vector3 &r_ps, Vector3 &r_qt);
+	static real_t get_closest_distance_between_segments(const Vector3 &p_p0, const Vector3 &p_p1, const Vector3 &p_q0, const Vector3 &p_q1);
+
+	static inline bool ray_intersects_triangle(const Vector3 &p_from, const Vector3 &p_dir, const Vector3 &p_v0, const Vector3 &p_v1, const Vector3 &p_v2, Vector3 *r_res = nullptr) {
+		Vector3 e1 = p_v1 - p_v0;
+		Vector3 e2 = p_v2 - p_v0;
+		Vector3 h = p_dir.cross(e2);
+		real_t a = e1.dot(h);
+		if (Math::is_zero_approx(a)) { // Parallel test.
+			return false;
+		}
+
+		real_t f = 1.0f / a;
+
+		Vector3 s = p_from - p_v0;
+		real_t u = f * s.dot(h);
+
+		if ((u < 0.0f) || (u > 1.0f)) {
+			return false;
+		}
+
+		Vector3 q = s.cross(e1);
+
+		real_t v = f * p_dir.dot(q);
+
+		if ((v < 0.0f) || (u + v > 1.0f)) {
+			return false;
+		}
+
+		// At this stage we can compute t to find out where
+		// the intersection point is on the line.
+		real_t t = f * e2.dot(q);
+
+		if (t > 0.00001f) { // ray intersection
+			if (r_res) {
+				*r_res = p_from + p_dir * t;
+			}
+			return true;
+		} else { // This means that there is a line intersection but not a ray intersection.
+			return false;
+		}
+	}
+
+	static inline bool segment_intersects_triangle(const Vector3 &p_from, const Vector3 &p_to, const Vector3 &p_v0, const Vector3 &p_v1, const Vector3 &p_v2, Vector3 *r_res = nullptr) {
+		Vector3 rel = p_to - p_from;
+		Vector3 e1 = p_v1 - p_v0;
+		Vector3 e2 = p_v2 - p_v0;
+		Vector3 h = rel.cross(e2);
+		real_t a = e1.dot(h);
+		if (Math::is_zero_approx(a)) { // Parallel test.
+			return false;
+		}
+
+		real_t f = 1.0f / a;
+
+		Vector3 s = p_from - p_v0;
+		real_t u = f * s.dot(h);
+
+		if ((u < 0.0f) || (u > 1.0f)) {
+			return false;
+		}
+
+		Vector3 q = s.cross(e1);
+
+		real_t v = f * rel.dot(q);
+
+		if ((v < 0.0f) || (u + v > 1.0f)) {
+			return false;
+		}
+
+		// At this stage we can compute t to find out where
+		// the intersection point is on the line.
+		real_t t = f * e2.dot(q);
+
+		if (t > (real_t)CMP_EPSILON && t <= 1.0f) { // Ray intersection.
+			if (r_res) {
+				*r_res = p_from + rel * t;
+			}
+			return true;
+		} else { // This means that there is a line intersection but not a ray intersection.
+			return false;
+		}
+	}
+
+	static inline bool segment_intersects_sphere(const Vector3 &p_from, const Vector3 &p_to, const Vector3 &p_sphere_pos, real_t p_sphere_radius, Vector3 *r_res = nullptr, Vector3 *r_norm = nullptr) {
+		Vector3 sphere_pos = p_sphere_pos - p_from;
+		Vector3 rel = (p_to - p_from);
+		real_t rel_l = rel.length();
+		if (rel_l < (real_t)CMP_EPSILON) {
+			return false; // Both points are the same.
+		}
+		Vector3 normal = rel / rel_l;
+
+		real_t sphere_d = normal.dot(sphere_pos);
+
+		real_t ray_distance = sphere_pos.distance_to(normal * sphere_d);
+
+		if (ray_distance >= p_sphere_radius) {
+			return false;
+		}
+
+		real_t inters_d2 = p_sphere_radius * p_sphere_radius - ray_distance * ray_distance;
+		real_t inters_d = sphere_d;
+
+		if (inters_d2 >= (real_t)CMP_EPSILON) {
+			inters_d -= Math::sqrt(inters_d2);
+		}
+
+		// Check in segment.
+		if (inters_d < 0 || inters_d > rel_l) {
+			return false;
+		}
+
+		Vector3 result = p_from + normal * inters_d;
+
+		if (r_res) {
+			*r_res = result;
+		}
+		if (r_norm) {
+			*r_norm = (result - p_sphere_pos).normalized();
+		}
+
+		return true;
+	}
+
+	static inline bool segment_intersects_cylinder(const Vector3 &p_from, const Vector3 &p_to, real_t p_height, real_t p_radius, Vector3 *r_res = nullptr, Vector3 *r_norm = nullptr, int p_cylinder_axis = 2) {
+		Vector3 rel = (p_to - p_from);
+		real_t rel_l = rel.length();
+		if (rel_l < (real_t)CMP_EPSILON) {
+			return false; // Both points are the same.
+		}
+
+		ERR_FAIL_COND_V(p_cylinder_axis < 0, false);
+		ERR_FAIL_COND_V(p_cylinder_axis > 2, false);
+		Vector3 cylinder_axis;
+		cylinder_axis[p_cylinder_axis] = 1.0f;
+
+		// First check if they are parallel.
+		Vector3 normal = (rel / rel_l);
+		Vector3 crs = normal.cross(cylinder_axis);
+		real_t crs_l = crs.length();
+
+		Vector3 axis_dir;
+
+		if (crs_l < (real_t)CMP_EPSILON) {
+			Vector3 side_axis;
+			side_axis[(p_cylinder_axis + 1) % 3] = 1.0f; // Any side axis OK.
+			axis_dir = side_axis;
+		} else {
+			axis_dir = crs / crs_l;
+		}
+
+		real_t dist = axis_dir.dot(p_from);
+
+		if (dist >= p_radius) {
+			return false; // Too far away.
+		}
+
+		// Convert to 2D.
+		real_t w2 = p_radius * p_radius - dist * dist;
+		if (w2 < (real_t)CMP_EPSILON) {
+			return false; // Avoid numerical error.
+		}
+		Size2 size(Math::sqrt(w2), p_height * 0.5f);
+
+		Vector3 side_dir = axis_dir.cross(cylinder_axis).normalized();
+
+		Vector2 from2D(side_dir.dot(p_from), p_from[p_cylinder_axis]);
+		Vector2 to2D(side_dir.dot(p_to), p_to[p_cylinder_axis]);
+
+		real_t min = 0, max = 1;
+
+		int axis = -1;
+
+		for (int i = 0; i < 2; i++) {
+			real_t seg_from = from2D[i];
+			real_t seg_to = to2D[i];
+			real_t box_begin = -size[i];
+			real_t box_end = size[i];
+			real_t cmin, cmax;
+
+			if (seg_from < seg_to) {
+				if (seg_from > box_end || seg_to < box_begin) {
+					return false;
+				}
+				real_t length = seg_to - seg_from;
+				cmin = (seg_from < box_begin) ? ((box_begin - seg_from) / length) : 0;
+				cmax = (seg_to > box_end) ? ((box_end - seg_from) / length) : 1;
+
+			} else {
+				if (seg_to > box_end || seg_from < box_begin) {
+					return false;
+				}
+				real_t length = seg_to - seg_from;
+				cmin = (seg_from > box_end) ? (box_end - seg_from) / length : 0;
+				cmax = (seg_to < box_begin) ? (box_begin - seg_from) / length : 1;
+			}
+
+			if (cmin > min) {
+				min = cmin;
+				axis = i;
+			}
+			if (cmax < max) {
+				max = cmax;
+			}
+			if (max < min) {
+				return false;
+			}
+		}
+
+		// Convert to 3D again.
+		Vector3 result = p_from + (rel * min);
+		Vector3 res_normal = result;
+
+		if (axis == 0) {
+			res_normal[p_cylinder_axis] = 0;
+		} else {
+			int axis_side = (p_cylinder_axis + 1) % 3;
+			res_normal[axis_side] = 0;
+			axis_side = (axis_side + 1) % 3;
+			res_normal[axis_side] = 0;
+		}
+
+		res_normal.normalize();
+
+		if (r_res) {
+			*r_res = result;
+		}
+		if (r_norm) {
+			*r_norm = res_normal;
+		}
+
+		return true;
+	}
+
+	static bool segment_intersects_convex(const Vector3 &p_from, const Vector3 &p_to, const Plane *p_planes, int p_plane_count, Vector3 *r_res, Vector3 *r_norm) {
+		real_t min = -1e20, max = 1e20;
+
+		Vector3 rel = p_to - p_from;
+		real_t rel_l = rel.length();
+
+		if (rel_l < (real_t)CMP_EPSILON) {
+			return false;
+		}
+
+		Vector3 dir = rel / rel_l;
+
+		int min_index = -1;
+
+		for (int i = 0; i < p_plane_count; i++) {
+			const Plane &p = p_planes[i];
+
+			real_t den = p.normal.dot(dir);
+
+			if (Math::abs(den) <= (real_t)CMP_EPSILON) {
+				continue; // Ignore parallel plane.
+			}
+
+			real_t dist = -p.distance_to(p_from) / den;
+
+			if (den > 0) {
+				// Backwards facing plane.
+				if (dist < max) {
+					max = dist;
+				}
+			} else {
+				// Front facing plane.
+				if (dist > min) {
+					min = dist;
+					min_index = i;
+				}
+			}
+		}
+
+		if (max <= min || min < 0 || min > rel_l || min_index == -1) { // Exit conditions.
+			return false; // No intersection.
+		}
+
+		if (r_res) {
+			*r_res = p_from + dir * min;
+		}
+		if (r_norm) {
+			*r_norm = p_planes[min_index].normal;
+		}
+
+		return true;
+	}
+
+#ifndef DISABLE_DEPRECATED
+	static Vector3 get_closest_point_to_segment(const Vector3 &p_point, const Vector3 *p_segment) {
+		return get_closest_point_to_segment(p_point, p_segment[0], p_segment[1]);
+	}
+#endif // DISABLE_DEPRECATED
+
+	static Vector3 get_closest_point_to_segment(const Vector3 &p_point, const Vector3 &p_segment_a, const Vector3 &p_segment_b) {
+		Vector3 p = p_point - p_segment_a;
+		Vector3 n = p_segment_b - p_segment_a;
+		real_t l2 = n.length_squared();
+		if (l2 < 1e-20f) {
+			return p_segment_a; // Both points are the same, just give any.
+		}
+
+		real_t d = n.dot(p) / l2;
+
+		if (d <= 0.0f) {
+			return p_segment_a; // Before first point.
+		} else if (d >= 1.0f) {
+			return p_segment_b; // After first point.
+		} else {
+			return p_segment_a + n * d; // Inside.
+		}
+	}
+
+#ifndef DISABLE_DEPRECATED
+	static Vector3 get_closest_point_to_segment_uncapped(const Vector3 &p_point, const Vector3 *p_segment) {
+		return get_closest_point_to_segment_uncapped(p_point, p_segment[0], p_segment[1]);
+	}
+#endif // DISABLE_DEPRECATED
+
+	static Vector3 get_closest_point_to_segment_uncapped(const Vector3 &p_point, const Vector3 &p_segment_a, const Vector3 &p_segment_b) {
+		Vector3 p = p_point - p_segment_a;
+		Vector3 n = p_segment_b - p_segment_a;
+		real_t l2 = n.length_squared();
+		if (l2 < 1e-20f) {
+			return p_segment_a; // Both points are the same, just give any.
+		}
+
+		real_t d = n.dot(p) / l2;
+
+		return p_segment_a + n * d; // Inside.
+	}
+
+	static inline bool point_in_projected_triangle(const Vector3 &p_point, const Vector3 &p_v1, const Vector3 &p_v2, const Vector3 &p_v3) {
+		Vector3 face_n = (p_v1 - p_v3).cross(p_v1 - p_v2);
+
+		Vector3 n1 = (p_point - p_v3).cross(p_point - p_v2);
+
+		if (face_n.dot(n1) < 0) {
+			return false;
+		}
+
+		Vector3 n2 = (p_v1 - p_v3).cross(p_v1 - p_point);
+
+		if (face_n.dot(n2) < 0) {
+			return false;
+		}
+
+		Vector3 n3 = (p_v1 - p_point).cross(p_v1 - p_v2);
+
+		if (face_n.dot(n3) < 0) {
+			return false;
+		}
+
+		return true;
+	}
+
+#ifndef DISABLE_DEPRECATED
+	static inline bool triangle_sphere_intersection_test(const Vector3 *p_triangle, const Vector3 &p_normal, const Vector3 &p_sphere_pos, real_t p_sphere_radius, Vector3 &r_triangle_contact, Vector3 &r_sphere_contact) {
+		return triangle_sphere_intersection_test(p_triangle[0], p_triangle[1], p_triangle[2], p_normal, p_sphere_pos, p_sphere_radius, r_triangle_contact, r_sphere_contact);
+	}
+#endif // DISABLE_DEPRECATED
+
+	static inline bool triangle_sphere_intersection_test(const Vector3 &p_triangle_a, const Vector3 &p_triangle_b, const Vector3 &p_triangle_c, const Vector3 &p_normal, const Vector3 &p_sphere_pos, real_t p_sphere_radius, Vector3 &r_triangle_contact, Vector3 &r_sphere_contact) {
+		real_t d = p_normal.dot(p_sphere_pos) - p_normal.dot(p_triangle_a);
+
+		if (d > p_sphere_radius || d < -p_sphere_radius) {
+			// Not touching the plane of the face, return.
+			return false;
+		}
+
+		Vector3 contact = p_sphere_pos - (p_normal * d);
+
+		/** 2nd) TEST INSIDE TRIANGLE **/
+
+		if (Geometry3D::point_in_projected_triangle(contact, p_triangle_a, p_triangle_b, p_triangle_c)) {
+			r_triangle_contact = contact;
+			r_sphere_contact = p_sphere_pos - p_normal * p_sphere_radius;
+			//printf("solved inside triangle\n");
+			return true;
+		}
+
+		/** 3rd TEST INSIDE EDGE CYLINDERS **/
+
+		const Vector3 verts[4] = { p_triangle_a, p_triangle_b, p_triangle_c, p_triangle_a }; // for() friendly
+
+		for (int i = 0; i < 3; i++) {
+			// Check edge cylinder.
+
+			Vector3 n1 = verts[i] - verts[i + 1];
+			Vector3 n2 = p_sphere_pos - verts[i + 1];
+
+			///@TODO Maybe discard by range here to make the algorithm quicker.
+
+			// Check point within cylinder radius.
+			Vector3 axis = n1.cross(n2).cross(n1);
+			axis.normalize();
+
+			real_t ad = axis.dot(n2);
+
+			if (Math::abs(ad) > p_sphere_radius) {
+				// No chance with this edge, too far away.
+				continue;
+			}
+
+			// Check point within edge capsule cylinder.
+			/** 4th TEST INSIDE EDGE POINTS **/
+
+			real_t sphere_at = n1.dot(n2);
+
+			if (sphere_at >= 0 && sphere_at < n1.dot(n1)) {
+				r_triangle_contact = p_sphere_pos - axis * (axis.dot(n2));
+				r_sphere_contact = p_sphere_pos - axis * p_sphere_radius;
+				// Point inside here.
+				return true;
+			}
+
+			real_t r2 = p_sphere_radius * p_sphere_radius;
+
+			if (n2.length_squared() < r2) {
+				Vector3 n = (p_sphere_pos - verts[i + 1]).normalized();
+
+				r_triangle_contact = verts[i + 1];
+				r_sphere_contact = p_sphere_pos - n * p_sphere_radius;
+				return true;
+			}
+
+			if (n2.distance_squared_to(n1) < r2) {
+				Vector3 n = (p_sphere_pos - verts[i]).normalized();
+
+				r_triangle_contact = verts[i];
+				r_sphere_contact = p_sphere_pos - n * p_sphere_radius;
+				return true;
+			}
+
+			break; // It's pointless to continue at this point, so save some CPU cycles.
+		}
+
+		return false;
+	}
+
+	static inline Vector<Vector3> clip_polygon(const Vector<Vector3> &polygon, const Plane &p_plane) {
+		enum LocationCache {
+			LOC_INSIDE = 1,
+			LOC_BOUNDARY = 0,
+			LOC_OUTSIDE = -1
+		};
+
+		if (polygon.is_empty()) {
+			return polygon;
+		}
+
+		int *location_cache = (int *)alloca(sizeof(int) * polygon.size());
+		int inside_count = 0;
+		int outside_count = 0;
+
+		for (int a = 0; a < polygon.size(); a++) {
+			real_t dist = p_plane.distance_to(polygon[a]);
+			if (dist < (real_t)-CMP_POINT_IN_PLANE_EPSILON) {
+				location_cache[a] = LOC_INSIDE;
+				inside_count++;
+			} else {
+				if (dist > (real_t)CMP_POINT_IN_PLANE_EPSILON) {
+					location_cache[a] = LOC_OUTSIDE;
+					outside_count++;
+				} else {
+					location_cache[a] = LOC_BOUNDARY;
+				}
+			}
+		}
+
+		if (outside_count == 0) {
+			return polygon; // No changes.
+		} else if (inside_count == 0) {
+			return Vector<Vector3>(); // Empty.
+		}
+
+		long previous = polygon.size() - 1;
+		Vector<Vector3> clipped;
+
+		for (int index = 0; index < polygon.size(); index++) {
+			int loc = location_cache[index];
+			if (loc == LOC_OUTSIDE) {
+				if (location_cache[previous] == LOC_INSIDE) {
+					const Vector3 &v1 = polygon[previous];
+					const Vector3 &v2 = polygon[index];
+
+					Vector3 segment = v1 - v2;
+					real_t den = p_plane.normal.dot(segment);
+					real_t dist = p_plane.distance_to(v1) / den;
+					dist = -dist;
+					clipped.push_back(v1 + segment * dist);
+				}
+			} else {
+				const Vector3 &v1 = polygon[index];
+				if ((loc == LOC_INSIDE) && (location_cache[previous] == LOC_OUTSIDE)) {
+					const Vector3 &v2 = polygon[previous];
+					Vector3 segment = v1 - v2;
+					real_t den = p_plane.normal.dot(segment);
+					real_t dist = p_plane.distance_to(v1) / den;
+					dist = -dist;
+					clipped.push_back(v1 + segment * dist);
+				}
+
+				clipped.push_back(v1);
+			}
+
+			previous = index;
+		}
+
+		return clipped;
+	}
+
+	static Vector<int32_t> tetrahedralize_delaunay(const Vector<Vector3> &p_points) {
+		Vector<Delaunay3D::OutputSimplex> tetr = Delaunay3D::tetrahedralize(p_points);
+		Vector<int32_t> tetrahedrons;
+
+		tetrahedrons.resize(4 * tetr.size());
+		int32_t *ptr = tetrahedrons.ptrw();
+		for (int i = 0; i < tetr.size(); i++) {
+			*ptr++ = tetr[i].points[0];
+			*ptr++ = tetr[i].points[1];
+			*ptr++ = tetr[i].points[2];
+			*ptr++ = tetr[i].points[3];
+		}
+		return tetrahedrons;
+	}
+
+	// Create a "wrap" that encloses the given geometry.
+	static Vector<Face3> wrap_geometry(const Vector<Face3> &p_array, real_t *p_error = nullptr);
+
+	struct MeshData {
+		struct Face {
+			Plane plane;
+			LocalVector<int> indices;
+		};
+
+		LocalVector<Face> faces;
+
+		struct Edge {
+			int vertex_a, vertex_b;
+			int face_a, face_b;
+		};
+
+		LocalVector<Edge> edges;
+
+		LocalVector<Vector3> vertices;
+
+		void optimize_vertices();
+	};
+
+	static MeshData build_convex_mesh(const Vector<Plane> &p_planes);
+	static Vector<Plane> build_sphere_planes(real_t p_radius, int p_lats, int p_lons, Vector3::Axis p_axis = Vector3::AXIS_Z);
+	static Vector<Plane> build_box_planes(const Vector3 &p_extents);
+	static Vector<Plane> build_cylinder_planes(real_t p_radius, real_t p_height, int p_sides, Vector3::Axis p_axis = Vector3::AXIS_Z);
+	static Vector<Plane> build_capsule_planes(real_t p_radius, real_t p_height, int p_sides, int p_lats, Vector3::Axis p_axis = Vector3::AXIS_Z);
+
+	static Vector<Vector3> compute_convex_mesh_points(const Plane *p_planes, int p_plane_count);
+
+#define FINDMINMAX(x0, x1, x2, min, max) \
+	min = max = x0;                      \
+	if (x1 < min) {                      \
+		min = x1;                        \
+	}                                    \
+	if (x1 > max) {                      \
+		max = x1;                        \
+	}                                    \
+	if (x2 < min) {                      \
+		min = x2;                        \
+	}                                    \
+	if (x2 > max) {                      \
+		max = x2;                        \
+	}
+
+	_FORCE_INLINE_ static bool planeBoxOverlap(Vector3 normal, real_t d, Vector3 maxbox) {
+		int q;
+		Vector3 vmin, vmax;
+		for (q = 0; q <= 2; q++) {
+			if (normal[q] > 0.0f) {
+				vmin[q] = -maxbox[q];
+				vmax[q] = maxbox[q];
+			} else {
+				vmin[q] = maxbox[q];
+				vmax[q] = -maxbox[q];
+			}
+		}
+		if (normal.dot(vmin) + d > 0.0f) {
+			return false;
+		}
+		if (normal.dot(vmax) + d >= 0.0f) {
+			return true;
+		}
+
+		return false;
+	}
+
+/*======================== X-tests ========================*/
+#define AXISTEST_X01(a, b, fa, fb)                 \
+	p0 = a * v0.y - b * v0.z;                      \
+	p2 = a * v2.y - b * v2.z;                      \
+	if (p0 < p2) {                                 \
+		min = p0;                                  \
+		max = p2;                                  \
+	} else {                                       \
+		min = p2;                                  \
+		max = p0;                                  \
+	}                                              \
+	rad = fa * boxhalfsize.y + fb * boxhalfsize.z; \
+	if (min > rad || max < -rad) {                 \
+		return false;                              \
+	}
+
+#define AXISTEST_X2(a, b, fa, fb)                  \
+	p0 = a * v0.y - b * v0.z;                      \
+	p1 = a * v1.y - b * v1.z;                      \
+	if (p0 < p1) {                                 \
+		min = p0;                                  \
+		max = p1;                                  \
+	} else {                                       \
+		min = p1;                                  \
+		max = p0;                                  \
+	}                                              \
+	rad = fa * boxhalfsize.y + fb * boxhalfsize.z; \
+	if (min > rad || max < -rad) {                 \
+		return false;                              \
+	}
+
+/*======================== Y-tests ========================*/
+#define AXISTEST_Y02(a, b, fa, fb)                 \
+	p0 = -a * v0.x + b * v0.z;                     \
+	p2 = -a * v2.x + b * v2.z;                     \
+	if (p0 < p2) {                                 \
+		min = p0;                                  \
+		max = p2;                                  \
+	} else {                                       \
+		min = p2;                                  \
+		max = p0;                                  \
+	}                                              \
+	rad = fa * boxhalfsize.x + fb * boxhalfsize.z; \
+	if (min > rad || max < -rad) {                 \
+		return false;                              \
+	}
+
+#define AXISTEST_Y1(a, b, fa, fb)                  \
+	p0 = -a * v0.x + b * v0.z;                     \
+	p1 = -a * v1.x + b * v1.z;                     \
+	if (p0 < p1) {                                 \
+		min = p0;                                  \
+		max = p1;                                  \
+	} else {                                       \
+		min = p1;                                  \
+		max = p0;                                  \
+	}                                              \
+	rad = fa * boxhalfsize.x + fb * boxhalfsize.z; \
+	if (min > rad || max < -rad) {                 \
+		return false;                              \
+	}
+
+/*======================== Z-tests ========================*/
+#define AXISTEST_Z12(a, b, fa, fb)                 \
+	p1 = a * v1.x - b * v1.y;                      \
+	p2 = a * v2.x - b * v2.y;                      \
+	if (p2 < p1) {                                 \
+		min = p2;                                  \
+		max = p1;                                  \
+	} else {                                       \
+		min = p1;                                  \
+		max = p2;                                  \
+	}                                              \
+	rad = fa * boxhalfsize.x + fb * boxhalfsize.y; \
+	if (min > rad || max < -rad) {                 \
+		return false;                              \
+	}
+
+#define AXISTEST_Z0(a, b, fa, fb)                  \
+	p0 = a * v0.x - b * v0.y;                      \
+	p1 = a * v1.x - b * v1.y;                      \
+	if (p0 < p1) {                                 \
+		min = p0;                                  \
+		max = p1;                                  \
+	} else {                                       \
+		min = p1;                                  \
+		max = p0;                                  \
+	}                                              \
+	rad = fa * boxhalfsize.x + fb * boxhalfsize.y; \
+	if (min > rad || max < -rad) {                 \
+		return false;                              \
+	}
+
+	_FORCE_INLINE_ static bool triangle_box_overlap(const Vector3 &boxcenter, const Vector3 boxhalfsize, const Vector3 *triverts) {
+		/*    use separating axis theorem to test overlap between triangle and box */
+		/*    need to test for overlap in these directions: */
+		/*    1) the {x,y,z}-directions (actually, since we use the AABB of the triangle */
+		/*       we do not even need to test these) */
+		/*    2) normal of the triangle */
+		/*    3) crossproduct(edge from tri, {x,y,z}-directin) */
+		/*       this gives 3x3=9 more tests */
+		real_t min, max, p0, p1, p2, rad, fex, fey, fez;
+
+		/* This is the fastest branch on Sun */
+		/* move everything so that the boxcenter is in (0,0,0) */
+
+		const Vector3 v0 = triverts[0] - boxcenter;
+		const Vector3 v1 = triverts[1] - boxcenter;
+		const Vector3 v2 = triverts[2] - boxcenter;
+
+		/* compute triangle edges */
+		const Vector3 e0 = v1 - v0; /* tri edge 0 */
+		const Vector3 e1 = v2 - v1; /* tri edge 1 */
+		const Vector3 e2 = v0 - v2; /* tri edge 2 */
+
+		/* Bullet 3:  */
+		/*  test the 9 tests first (this was faster) */
+		fex = Math::abs(e0.x);
+		fey = Math::abs(e0.y);
+		fez = Math::abs(e0.z);
+		AXISTEST_X01(e0.z, e0.y, fez, fey);
+		AXISTEST_Y02(e0.z, e0.x, fez, fex);
+		AXISTEST_Z12(e0.y, e0.x, fey, fex);
+
+		fex = Math::abs(e1.x);
+		fey = Math::abs(e1.y);
+		fez = Math::abs(e1.z);
+		AXISTEST_X01(e1.z, e1.y, fez, fey);
+		AXISTEST_Y02(e1.z, e1.x, fez, fex);
+		AXISTEST_Z0(e1.y, e1.x, fey, fex);
+
+		fex = Math::abs(e2.x);
+		fey = Math::abs(e2.y);
+		fez = Math::abs(e2.z);
+		AXISTEST_X2(e2.z, e2.y, fez, fey);
+		AXISTEST_Y1(e2.z, e2.x, fez, fex);
+		AXISTEST_Z12(e2.y, e2.x, fey, fex);
+
+		/* Bullet 1: */
+		/*  first test overlap in the {x,y,z}-directions */
+		/*  find min, max of the triangle each direction, and test for overlap in */
+		/*  that direction -- this is equivalent to testing a minimal AABB around */
+		/*  the triangle against the AABB */
+
+		/* test in X-direction */
+		FINDMINMAX(v0.x, v1.x, v2.x, min, max);
+		if (min > boxhalfsize.x || max < -boxhalfsize.x) {
+			return false;
+		}
+
+		/* test in Y-direction */
+		FINDMINMAX(v0.y, v1.y, v2.y, min, max);
+		if (min > boxhalfsize.y || max < -boxhalfsize.y) {
+			return false;
+		}
+
+		/* test in Z-direction */
+		FINDMINMAX(v0.z, v1.z, v2.z, min, max);
+		if (min > boxhalfsize.z || max < -boxhalfsize.z) {
+			return false;
+		}
+
+		/* Bullet 2: */
+		/*  test if the box intersects the plane of the triangle */
+		/*  compute plane equation of triangle: normal*x+d=0 */
+		const Vector3 normal = e0.cross(e1);
+		const real_t d = -normal.dot(v0); /* plane eq: normal.x+d=0 */
+		return planeBoxOverlap(normal, d, boxhalfsize); /* if true, box and triangle overlaps */
+	}
+
+	static Vector<uint32_t> generate_edf(const Vector<bool> &p_voxels, const Vector3i &p_size, bool p_negative);
+	static Vector<int8_t> generate_sdf8(const Vector<uint32_t> &p_positive, const Vector<uint32_t> &p_negative);
+
+	static Vector3 triangle_get_barycentric_coords(const Vector3 &p_a, const Vector3 &p_b, const Vector3 &p_c, const Vector3 &p_pos) {
+		const Vector3 v0 = p_b - p_a;
+		const Vector3 v1 = p_c - p_a;
+		const Vector3 v2 = p_pos - p_a;
+
+		const real_t d00 = v0.dot(v0);
+		const real_t d01 = v0.dot(v1);
+		const real_t d11 = v1.dot(v1);
+		const real_t d20 = v2.dot(v0);
+		const real_t d21 = v2.dot(v1);
+		const real_t denom = (d00 * d11 - d01 * d01);
+		if (denom == 0) {
+			return Vector3(); //invalid triangle, return empty
+		}
+		const real_t v = (d11 * d20 - d01 * d21) / denom;
+		const real_t w = (d00 * d21 - d01 * d20) / denom;
+		const real_t u = 1.0f - v - w;
+		return Vector3(u, v, w);
+	}
+
+	static Color tetrahedron_get_barycentric_coords(const Vector3 &p_a, const Vector3 &p_b, const Vector3 &p_c, const Vector3 &p_d, const Vector3 &p_pos) {
+		const Vector3 vap = p_pos - p_a;
+		const Vector3 vbp = p_pos - p_b;
+
+		const Vector3 vab = p_b - p_a;
+		const Vector3 vac = p_c - p_a;
+		const Vector3 vad = p_d - p_a;
+
+		const Vector3 vbc = p_c - p_b;
+		const Vector3 vbd = p_d - p_b;
+		// ScTP computes the scalar triple product
+#define STP(m_a, m_b, m_c) ((m_a).dot((m_b).cross((m_c))))
+		const real_t va6 = STP(vbp, vbd, vbc);
+		const real_t vb6 = STP(vap, vac, vad);
+		const real_t vc6 = STP(vap, vad, vab);
+		const real_t vd6 = STP(vap, vab, vac);
+		const real_t v6 = 1 / STP(vab, vac, vad);
+		return Color(va6 * v6, vb6 * v6, vc6 * v6, vd6 * v6);
+#undef STP
+	}
+
+	_FORCE_INLINE_ static Vector3 octahedron_map_decode(const Vector2 &p_uv) {
+		// https://twitter.com/Stubbesaurus/status/937994790553227264
+		const Vector2 f = p_uv * 2.0f - Vector2(1.0f, 1.0f);
+		Vector3 n = Vector3(f.x, f.y, 1.0f - Math::abs(f.x) - Math::abs(f.y));
+		const real_t 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();
+	}
+};

core/math/math_defs.h

@@ -0,0 +1,147 @@
+/**************************************************************************/
+/*  math_defs.h                                                           */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "core/typedefs.h"
+
+#include <limits>
+
+namespace Math {
+inline constexpr double SQRT2 = 1.4142135623730950488016887242;
+inline constexpr double SQRT3 = 1.7320508075688772935274463415059;
+inline constexpr double SQRT12 = 0.7071067811865475244008443621048490;
+inline constexpr double SQRT13 = 0.57735026918962576450914878050196;
+inline constexpr double LN2 = 0.6931471805599453094172321215;
+inline constexpr double TAU = 6.2831853071795864769252867666;
+inline constexpr double PI = 3.1415926535897932384626433833;
+inline constexpr double E = 2.7182818284590452353602874714;
+inline constexpr double INF = std::numeric_limits<double>::infinity();
+inline constexpr double NaN = std::numeric_limits<double>::quiet_NaN();
+} // namespace Math
+
+#define CMP_EPSILON 0.00001
+#define CMP_EPSILON2 (CMP_EPSILON * CMP_EPSILON)
+
+#define CMP_NORMALIZE_TOLERANCE 0.000001
+#define CMP_POINT_IN_PLANE_EPSILON 0.00001
+
+#ifdef DEBUG_ENABLED
+#define MATH_CHECKS
+#endif
+
+//this epsilon is for values related to a unit size (scalar or vector len)
+#ifdef PRECISE_MATH_CHECKS
+#define UNIT_EPSILON 0.00001
+#else
+//tolerate some more floating point error normally
+#define UNIT_EPSILON 0.001
+#endif
+
+#define USEC_TO_SEC(m_usec) ((m_usec) / 1000000.0)
+
+enum ClockDirection {
+	CLOCKWISE,
+	COUNTERCLOCKWISE
+};
+
+enum Orientation {
+	HORIZONTAL,
+	VERTICAL
+};
+
+enum HorizontalAlignment {
+	HORIZONTAL_ALIGNMENT_LEFT,
+	HORIZONTAL_ALIGNMENT_CENTER,
+	HORIZONTAL_ALIGNMENT_RIGHT,
+	HORIZONTAL_ALIGNMENT_FILL,
+};
+
+enum VerticalAlignment {
+	VERTICAL_ALIGNMENT_TOP,
+	VERTICAL_ALIGNMENT_CENTER,
+	VERTICAL_ALIGNMENT_BOTTOM,
+	VERTICAL_ALIGNMENT_FILL,
+};
+
+enum InlineAlignment {
+	// Image alignment points.
+	INLINE_ALIGNMENT_TOP_TO = 0b0000,
+	INLINE_ALIGNMENT_CENTER_TO = 0b0001,
+	INLINE_ALIGNMENT_BASELINE_TO = 0b0011,
+	INLINE_ALIGNMENT_BOTTOM_TO = 0b0010,
+	INLINE_ALIGNMENT_IMAGE_MASK = 0b0011,
+
+	// Text alignment points.
+	INLINE_ALIGNMENT_TO_TOP = 0b0000,
+	INLINE_ALIGNMENT_TO_CENTER = 0b0100,
+	INLINE_ALIGNMENT_TO_BASELINE = 0b1000,
+	INLINE_ALIGNMENT_TO_BOTTOM = 0b1100,
+	INLINE_ALIGNMENT_TEXT_MASK = 0b1100,
+
+	// Presets.
+	INLINE_ALIGNMENT_TOP = INLINE_ALIGNMENT_TOP_TO | INLINE_ALIGNMENT_TO_TOP,
+	INLINE_ALIGNMENT_CENTER = INLINE_ALIGNMENT_CENTER_TO | INLINE_ALIGNMENT_TO_CENTER,
+	INLINE_ALIGNMENT_BOTTOM = INLINE_ALIGNMENT_BOTTOM_TO | INLINE_ALIGNMENT_TO_BOTTOM
+};
+
+enum Side {
+	SIDE_LEFT,
+	SIDE_TOP,
+	SIDE_RIGHT,
+	SIDE_BOTTOM
+};
+
+enum Corner {
+	CORNER_TOP_LEFT,
+	CORNER_TOP_RIGHT,
+	CORNER_BOTTOM_RIGHT,
+	CORNER_BOTTOM_LEFT
+};
+
+enum class EulerOrder {
+	XYZ,
+	XZY,
+	YXZ,
+	YZX,
+	ZXY,
+	ZYX
+};
+
+/**
+ * The "Real" type is an abstract type used for real numbers, such as 1.5,
+ * in contrast to integer numbers. Precision can be controlled with the
+ * presence or absence of the REAL_T_IS_DOUBLE define.
+ */
+#ifdef REAL_T_IS_DOUBLE
+typedef double real_t;
+#else
+typedef float real_t;
+#endif

core/math/math_fieldwise.cpp

@@ -0,0 +1,245 @@
+/**************************************************************************/
+/*  math_fieldwise.cpp                                                    */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#ifdef DEBUG_ENABLED
+
+#include "math_fieldwise.h"
+
+#define SETUP_TYPE(m_type)    \
+	m_type source = p_source; \
+	m_type target = p_target;
+#define TRY_TRANSFER_FIELD(m_name, m_member) \
+	if (p_field == m_name) {                 \
+		target.m_member = source.m_member;   \
+	}
+
+Variant fieldwise_assign(const Variant &p_target, const Variant &p_source, const String &p_field) {
+	ERR_FAIL_COND_V(p_target.get_type() != p_source.get_type(), p_target);
+
+	/* clang-format makes a mess of this macro usage */
+	/* clang-format off */
+
+	switch (p_source.get_type()) {
+		case Variant::VECTOR2: {
+			SETUP_TYPE(Vector2)
+
+			/**/ TRY_TRANSFER_FIELD("x", x)
+			else TRY_TRANSFER_FIELD("y", y)
+
+			return target;
+		}
+
+		case Variant::VECTOR2I: {
+			SETUP_TYPE(Vector2i)
+
+			/**/ TRY_TRANSFER_FIELD("x", x)
+			else TRY_TRANSFER_FIELD("y", y)
+
+			return target;
+		}
+
+		case Variant::RECT2: {
+			SETUP_TYPE(Rect2)
+
+			/**/ TRY_TRANSFER_FIELD("x", position.x)
+			else TRY_TRANSFER_FIELD("y", position.y)
+			else TRY_TRANSFER_FIELD("w", size.x)
+			else TRY_TRANSFER_FIELD("h", size.y)
+
+			return target;
+		}
+
+		case Variant::RECT2I: {
+			SETUP_TYPE(Rect2i)
+
+			/**/ TRY_TRANSFER_FIELD("x", position.x)
+			else TRY_TRANSFER_FIELD("y", position.y)
+			else TRY_TRANSFER_FIELD("w", size.x)
+			else TRY_TRANSFER_FIELD("h", size.y)
+
+			return target;
+		}
+
+		case Variant::VECTOR3: {
+			SETUP_TYPE(Vector3)
+
+			/**/ TRY_TRANSFER_FIELD("x", x)
+			else TRY_TRANSFER_FIELD("y", y)
+			else TRY_TRANSFER_FIELD("z", z)
+
+			return target;
+		}
+
+		case Variant::VECTOR3I: {
+			SETUP_TYPE(Vector3i)
+
+			/**/ TRY_TRANSFER_FIELD("x", x)
+			else TRY_TRANSFER_FIELD("y", y)
+			else TRY_TRANSFER_FIELD("z", z)
+
+			return target;
+		}
+
+		case Variant::VECTOR4: {
+			SETUP_TYPE(Vector4)
+
+			/**/ TRY_TRANSFER_FIELD("x", x)
+			else TRY_TRANSFER_FIELD("y", y)
+			else TRY_TRANSFER_FIELD("z", z)
+			else TRY_TRANSFER_FIELD("w", w)
+
+			return target;
+		}
+
+		case Variant::VECTOR4I: {
+			SETUP_TYPE(Vector4i)
+
+			/**/ TRY_TRANSFER_FIELD("x", x)
+			else TRY_TRANSFER_FIELD("y", y)
+			else TRY_TRANSFER_FIELD("z", z)
+			else TRY_TRANSFER_FIELD("w", w)
+
+			return target;
+		}
+
+		case Variant::PLANE: {
+			SETUP_TYPE(Plane)
+
+			/**/ TRY_TRANSFER_FIELD("x", normal.x)
+			else TRY_TRANSFER_FIELD("y", normal.y)
+			else TRY_TRANSFER_FIELD("z", normal.z)
+			else TRY_TRANSFER_FIELD("d", d)
+
+			return target;
+		}
+
+		case Variant::QUATERNION: {
+			SETUP_TYPE(Quaternion)
+
+			/**/ TRY_TRANSFER_FIELD("x", x)
+			else TRY_TRANSFER_FIELD("y", y)
+			else TRY_TRANSFER_FIELD("z", z)
+			else TRY_TRANSFER_FIELD("w", w)
+
+			return target;
+		}
+
+		case Variant::AABB: {
+			SETUP_TYPE(AABB)
+
+			/**/ TRY_TRANSFER_FIELD("px", position.x)
+			else TRY_TRANSFER_FIELD("py", position.y)
+			else TRY_TRANSFER_FIELD("pz", position.z)
+			else TRY_TRANSFER_FIELD("sx", size.x)
+			else TRY_TRANSFER_FIELD("sy", size.y)
+			else TRY_TRANSFER_FIELD("sz", size.z)
+
+			return target;
+		}
+
+		case Variant::TRANSFORM2D: {
+			SETUP_TYPE(Transform2D)
+
+			/**/ TRY_TRANSFER_FIELD("xx", columns[0][0])
+			else TRY_TRANSFER_FIELD("xy", columns[0][1])
+			else TRY_TRANSFER_FIELD("yx", columns[1][0])
+			else TRY_TRANSFER_FIELD("yy", columns[1][1])
+			else TRY_TRANSFER_FIELD("ox", columns[2][0])
+			else TRY_TRANSFER_FIELD("oy", columns[2][1])
+
+			return target;
+		}
+
+		case Variant::BASIS: {
+			SETUP_TYPE(Basis)
+
+			/**/ TRY_TRANSFER_FIELD("xx", rows[0][0])
+			else TRY_TRANSFER_FIELD("xy", rows[0][1])
+			else TRY_TRANSFER_FIELD("xz", rows[0][2])
+			else TRY_TRANSFER_FIELD("yx", rows[1][0])
+			else TRY_TRANSFER_FIELD("yy", rows[1][1])
+			else TRY_TRANSFER_FIELD("yz", rows[1][2])
+			else TRY_TRANSFER_FIELD("zx", rows[2][0])
+			else TRY_TRANSFER_FIELD("zy", rows[2][1])
+			else TRY_TRANSFER_FIELD("zz", rows[2][2])
+
+			return target;
+		}
+
+		case Variant::TRANSFORM3D: {
+			SETUP_TYPE(Transform3D)
+
+			/**/ TRY_TRANSFER_FIELD("xx", basis.rows[0][0])
+			else TRY_TRANSFER_FIELD("xy", basis.rows[0][1])
+			else TRY_TRANSFER_FIELD("xz", basis.rows[0][2])
+			else TRY_TRANSFER_FIELD("yx", basis.rows[1][0])
+			else TRY_TRANSFER_FIELD("yy", basis.rows[1][1])
+			else TRY_TRANSFER_FIELD("yz", basis.rows[1][2])
+			else TRY_TRANSFER_FIELD("zx", basis.rows[2][0])
+			else TRY_TRANSFER_FIELD("zy", basis.rows[2][1])
+			else TRY_TRANSFER_FIELD("zz", basis.rows[2][2])
+			else TRY_TRANSFER_FIELD("xo", origin.x)
+			else TRY_TRANSFER_FIELD("yo", origin.y)
+			else TRY_TRANSFER_FIELD("zo", origin.z)
+
+			return target;
+		}
+
+		case Variant::PROJECTION: {
+			SETUP_TYPE(Projection)
+
+			/**/ TRY_TRANSFER_FIELD("xx", columns[0].x)
+			else TRY_TRANSFER_FIELD("xy", columns[0].y)
+			else TRY_TRANSFER_FIELD("xz", columns[0].z)
+			else TRY_TRANSFER_FIELD("xw", columns[0].w)
+			else TRY_TRANSFER_FIELD("yx", columns[1].x)
+			else TRY_TRANSFER_FIELD("yy", columns[1].y)
+			else TRY_TRANSFER_FIELD("yz", columns[1].z)
+			else TRY_TRANSFER_FIELD("yw", columns[1].w)
+			else TRY_TRANSFER_FIELD("zx", columns[2].x)
+			else TRY_TRANSFER_FIELD("zy", columns[2].y)
+			else TRY_TRANSFER_FIELD("zz", columns[2].z)
+			else TRY_TRANSFER_FIELD("zw", columns[2].w)
+			else TRY_TRANSFER_FIELD("xo", columns[3].x)
+			else TRY_TRANSFER_FIELD("yo", columns[3].y)
+			else TRY_TRANSFER_FIELD("zo", columns[3].z)
+			else TRY_TRANSFER_FIELD("wo", columns[3].w)
+
+			return target;
+		}
+
+		default: {
+			ERR_FAIL_V(p_target);
+		}
+	}
+	/* clang-format on */
+}
+
+#endif // DEBUG_ENABLED

core/math/math_fieldwise.h

@@ -0,0 +1,39 @@
+/**************************************************************************/
+/*  math_fieldwise.h                                                      */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#pragma once
+
+#ifdef DEBUG_ENABLED
+
+#include "core/variant/variant.h"
+
+Variant fieldwise_assign(const Variant &p_target, const Variant &p_source, const String &p_field);
+
+#endif // DEBUG_ENABLED

core/math/math_funcs.cpp

@@ -0,0 +1,182 @@
+/**************************************************************************/
+/*  math_funcs.cpp                                                        */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "math_funcs.h"
+
+#include "core/error/error_macros.h"
+#include "core/math/random_pcg.h"
+
+static RandomPCG default_rand;
+
+uint32_t Math::rand_from_seed(uint64_t *p_seed) {
+	RandomPCG rng = RandomPCG(*p_seed);
+	uint32_t r = rng.rand();
+	*p_seed = rng.get_seed();
+	return r;
+}
+
+void Math::seed(uint64_t p_value) {
+	default_rand.seed(p_value);
+}
+
+void Math::randomize() {
+	default_rand.randomize();
+}
+
+uint32_t Math::rand() {
+	return default_rand.rand();
+}
+
+double Math::randfn(double p_mean, double p_deviation) {
+	return default_rand.randfn(p_mean, p_deviation);
+}
+
+int Math::step_decimals(double p_step) {
+	static const int maxn = 10;
+	static const double sd[maxn] = {
+		0.9999, // somehow compensate for floating point error
+		0.09999,
+		0.009999,
+		0.0009999,
+		0.00009999,
+		0.000009999,
+		0.0000009999,
+		0.00000009999,
+		0.000000009999,
+		0.0000000009999
+	};
+
+	double abs = Math::abs(p_step);
+	double decs = abs - (int)abs; // Strip away integer part
+	for (int i = 0; i < maxn; i++) {
+		if (decs >= sd[i]) {
+			return i;
+		}
+	}
+
+	return 0;
+}
+
+// Only meant for editor usage in float ranges, where a step of 0
+// means that decimal digits should not be limited in String::num.
+int Math::range_step_decimals(double p_step) {
+	if (p_step < 0.0000000000001) {
+		return 16; // Max value hardcoded in String::num
+	}
+	return step_decimals(p_step);
+}
+
+double Math::ease(double p_x, double p_c) {
+	if (p_x < 0) {
+		p_x = 0;
+	} else if (p_x > 1.0) {
+		p_x = 1.0;
+	}
+	if (p_c > 0) {
+		if (p_c < 1.0) {
+			return 1.0 - Math::pow(1.0 - p_x, 1.0 / p_c);
+		} else {
+			return Math::pow(p_x, p_c);
+		}
+	} else if (p_c < 0) {
+		//inout ease
+
+		if (p_x < 0.5) {
+			return Math::pow(p_x * 2.0, -p_c) * 0.5;
+		} else {
+			return (1.0 - Math::pow(1.0 - (p_x - 0.5) * 2.0, -p_c)) * 0.5 + 0.5;
+		}
+	} else {
+		return 0; // no ease (raw)
+	}
+}
+
+double Math::snapped(double p_value, double p_step) {
+	if (p_step != 0) {
+		p_value = Math::floor(p_value / p_step + 0.5) * p_step;
+	}
+	return p_value;
+}
+
+uint32_t Math::larger_prime(uint32_t p_val) {
+	static const uint32_t primes[] = {
+		5,
+		13,
+		23,
+		47,
+		97,
+		193,
+		389,
+		769,
+		1543,
+		3079,
+		6151,
+		12289,
+		24593,
+		49157,
+		98317,
+		196613,
+		393241,
+		786433,
+		1572869,
+		3145739,
+		6291469,
+		12582917,
+		25165843,
+		50331653,
+		100663319,
+		201326611,
+		402653189,
+		805306457,
+		1610612741,
+		0,
+	};
+
+	int idx = 0;
+	while (true) {
+		ERR_FAIL_COND_V(primes[idx] == 0, 0);
+		if (primes[idx] > p_val) {
+			return primes[idx];
+		}
+		idx++;
+	}
+}
+
+double Math::random(double p_from, double p_to) {
+	return default_rand.random(p_from, p_to);
+}
+
+float Math::random(float p_from, float p_to) {
+	return default_rand.random(p_from, p_to);
+}
+
+int Math::random(int p_from, int p_to) {
+	return default_rand.random(p_from, p_to);
+}

core/math/math_funcs.h

@@ -0,0 +1,814 @@
+/**************************************************************************/
+/*  math_funcs.h                                                          */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "core/error/error_macros.h"
+#include "core/math/math_defs.h"
+#include "core/typedefs.h"
+
+#include <cfloat>
+#include <cmath>
+
+namespace Math {
+
+_ALWAYS_INLINE_ double sin(double p_x) {
+	return std::sin(p_x);
+}
+_ALWAYS_INLINE_ float sin(float p_x) {
+	return std::sin(p_x);
+}
+
+_ALWAYS_INLINE_ double cos(double p_x) {
+	return std::cos(p_x);
+}
+_ALWAYS_INLINE_ float cos(float p_x) {
+	return std::cos(p_x);
+}
+
+_ALWAYS_INLINE_ double tan(double p_x) {
+	return std::tan(p_x);
+}
+_ALWAYS_INLINE_ float tan(float p_x) {
+	return std::tan(p_x);
+}
+
+_ALWAYS_INLINE_ double sinh(double p_x) {
+	return std::sinh(p_x);
+}
+_ALWAYS_INLINE_ float sinh(float p_x) {
+	return std::sinh(p_x);
+}
+
+_ALWAYS_INLINE_ double sinc(double p_x) {
+	return p_x == 0 ? 1 : sin(p_x) / p_x;
+}
+_ALWAYS_INLINE_ float sinc(float p_x) {
+	return p_x == 0 ? 1 : sin(p_x) / p_x;
+}
+
+_ALWAYS_INLINE_ double sincn(double p_x) {
+	return sinc(PI * p_x);
+}
+_ALWAYS_INLINE_ float sincn(float p_x) {
+	return sinc((float)PI * p_x);
+}
+
+_ALWAYS_INLINE_ double cosh(double p_x) {
+	return std::cosh(p_x);
+}
+_ALWAYS_INLINE_ float cosh(float p_x) {
+	return std::cosh(p_x);
+}
+
+_ALWAYS_INLINE_ double tanh(double p_x) {
+	return std::tanh(p_x);
+}
+_ALWAYS_INLINE_ float tanh(float p_x) {
+	return std::tanh(p_x);
+}
+
+// Always does clamping so always safe to use.
+_ALWAYS_INLINE_ double asin(double p_x) {
+	return p_x < -1 ? (-PI / 2) : (p_x > 1 ? (PI / 2) : std::asin(p_x));
+}
+_ALWAYS_INLINE_ float asin(float p_x) {
+	return p_x < -1 ? (-(float)PI / 2) : (p_x > 1 ? ((float)PI / 2) : std::asin(p_x));
+}
+
+// Always does clamping so always safe to use.
+_ALWAYS_INLINE_ double acos(double p_x) {
+	return p_x < -1 ? PI : (p_x > 1 ? 0 : std::acos(p_x));
+}
+_ALWAYS_INLINE_ float acos(float p_x) {
+	return p_x < -1 ? (float)PI : (p_x > 1 ? 0 : std::acos(p_x));
+}
+
+_ALWAYS_INLINE_ double atan(double p_x) {
+	return std::atan(p_x);
+}
+_ALWAYS_INLINE_ float atan(float p_x) {
+	return std::atan(p_x);
+}
+
+_ALWAYS_INLINE_ double atan2(double p_y, double p_x) {
+	return std::atan2(p_y, p_x);
+}
+_ALWAYS_INLINE_ float atan2(float p_y, float p_x) {
+	return std::atan2(p_y, p_x);
+}
+
+_ALWAYS_INLINE_ double asinh(double p_x) {
+	return std::asinh(p_x);
+}
+_ALWAYS_INLINE_ float asinh(float p_x) {
+	return std::asinh(p_x);
+}
+
+// Always does clamping so always safe to use.
+_ALWAYS_INLINE_ double acosh(double p_x) {
+	return p_x < 1 ? 0 : std::acosh(p_x);
+}
+_ALWAYS_INLINE_ float acosh(float p_x) {
+	return p_x < 1 ? 0 : std::acosh(p_x);
+}
+
+// Always does clamping so always safe to use.
+_ALWAYS_INLINE_ double atanh(double p_x) {
+	return p_x <= -1 ? -INF : (p_x >= 1 ? INF : std::atanh(p_x));
+}
+_ALWAYS_INLINE_ float atanh(float p_x) {
+	return p_x <= -1 ? (float)-INF : (p_x >= 1 ? (float)INF : std::atanh(p_x));
+}
+
+_ALWAYS_INLINE_ double sqrt(double p_x) {
+	return std::sqrt(p_x);
+}
+_ALWAYS_INLINE_ float sqrt(float p_x) {
+	return std::sqrt(p_x);
+}
+
+_ALWAYS_INLINE_ double fmod(double p_x, double p_y) {
+	return std::fmod(p_x, p_y);
+}
+_ALWAYS_INLINE_ float fmod(float p_x, float p_y) {
+	return std::fmod(p_x, p_y);
+}
+
+_ALWAYS_INLINE_ double modf(double p_x, double *r_y) {
+	return std::modf(p_x, r_y);
+}
+_ALWAYS_INLINE_ float modf(float p_x, float *r_y) {
+	return std::modf(p_x, r_y);
+}
+
+_ALWAYS_INLINE_ double floor(double p_x) {
+	return std::floor(p_x);
+}
+_ALWAYS_INLINE_ float floor(float p_x) {
+	return std::floor(p_x);
+}
+
+_ALWAYS_INLINE_ double ceil(double p_x) {
+	return std::ceil(p_x);
+}
+_ALWAYS_INLINE_ float ceil(float p_x) {
+	return std::ceil(p_x);
+}
+
+_ALWAYS_INLINE_ double pow(double p_x, double p_y) {
+	return std::pow(p_x, p_y);
+}
+_ALWAYS_INLINE_ float pow(float p_x, float p_y) {
+	return std::pow(p_x, p_y);
+}
+
+_ALWAYS_INLINE_ double log(double p_x) {
+	return std::log(p_x);
+}
+_ALWAYS_INLINE_ float log(float p_x) {
+	return std::log(p_x);
+}
+
+_ALWAYS_INLINE_ double log1p(double p_x) {
+	return std::log1p(p_x);
+}
+_ALWAYS_INLINE_ float log1p(float p_x) {
+	return std::log1p(p_x);
+}
+
+_ALWAYS_INLINE_ double log2(double p_x) {
+	return std::log2(p_x);
+}
+_ALWAYS_INLINE_ float log2(float p_x) {
+	return std::log2(p_x);
+}
+
+_ALWAYS_INLINE_ double exp(double p_x) {
+	return std::exp(p_x);
+}
+_ALWAYS_INLINE_ float exp(float p_x) {
+	return std::exp(p_x);
+}
+
+_ALWAYS_INLINE_ bool is_nan(double p_val) {
+	return std::isnan(p_val);
+}
+
+_ALWAYS_INLINE_ bool is_nan(float p_val) {
+	return std::isnan(p_val);
+}
+
+_ALWAYS_INLINE_ bool is_inf(double p_val) {
+	return std::isinf(p_val);
+}
+
+_ALWAYS_INLINE_ bool is_inf(float p_val) {
+	return std::isinf(p_val);
+}
+
+// These methods assume (p_num + p_den) doesn't overflow.
+_ALWAYS_INLINE_ int32_t division_round_up(int32_t p_num, int32_t p_den) {
+	int32_t offset = (p_num < 0 && p_den < 0) ? 1 : -1;
+	return (p_num + p_den + offset) / p_den;
+}
+_ALWAYS_INLINE_ uint32_t division_round_up(uint32_t p_num, uint32_t p_den) {
+	return (p_num + p_den - 1) / p_den;
+}
+_ALWAYS_INLINE_ int64_t division_round_up(int64_t p_num, int64_t p_den) {
+	int32_t offset = (p_num < 0 && p_den < 0) ? 1 : -1;
+	return (p_num + p_den + offset) / p_den;
+}
+_ALWAYS_INLINE_ uint64_t division_round_up(uint64_t p_num, uint64_t p_den) {
+	return (p_num + p_den - 1) / p_den;
+}
+
+_ALWAYS_INLINE_ bool is_finite(double p_val) {
+	return std::isfinite(p_val);
+}
+_ALWAYS_INLINE_ bool is_finite(float p_val) {
+	return std::isfinite(p_val);
+}
+
+_ALWAYS_INLINE_ double abs(double p_value) {
+	return std::abs(p_value);
+}
+_ALWAYS_INLINE_ float abs(float p_value) {
+	return std::abs(p_value);
+}
+_ALWAYS_INLINE_ int8_t abs(int8_t p_value) {
+	return p_value > 0 ? p_value : -p_value;
+}
+_ALWAYS_INLINE_ int16_t abs(int16_t p_value) {
+	return p_value > 0 ? p_value : -p_value;
+}
+_ALWAYS_INLINE_ int32_t abs(int32_t p_value) {
+	return std::abs(p_value);
+}
+_ALWAYS_INLINE_ int64_t abs(int64_t p_value) {
+	return std::abs(p_value);
+}
+
+_ALWAYS_INLINE_ double fposmod(double p_x, double p_y) {
+	double value = fmod(p_x, p_y);
+	if (((value < 0) && (p_y > 0)) || ((value > 0) && (p_y < 0))) {
+		value += p_y;
+	}
+	value += 0.0;
+	return value;
+}
+_ALWAYS_INLINE_ float fposmod(float p_x, float p_y) {
+	float value = fmod(p_x, p_y);
+	if (((value < 0) && (p_y > 0)) || ((value > 0) && (p_y < 0))) {
+		value += p_y;
+	}
+	value += 0.0f;
+	return value;
+}
+
+_ALWAYS_INLINE_ double fposmodp(double p_x, double p_y) {
+	double value = fmod(p_x, p_y);
+	if (value < 0) {
+		value += p_y;
+	}
+	value += 0.0;
+	return value;
+}
+_ALWAYS_INLINE_ float fposmodp(float p_x, float p_y) {
+	float value = fmod(p_x, p_y);
+	if (value < 0) {
+		value += p_y;
+	}
+	value += 0.0f;
+	return value;
+}
+
+_ALWAYS_INLINE_ int64_t posmod(int64_t p_x, int64_t p_y) {
+	ERR_FAIL_COND_V_MSG(p_y == 0, 0, "Division by zero in posmod is undefined. Returning 0 as fallback.");
+	int64_t value = p_x % p_y;
+	if (((value < 0) && (p_y > 0)) || ((value > 0) && (p_y < 0))) {
+		value += p_y;
+	}
+	return value;
+}
+
+_ALWAYS_INLINE_ double deg_to_rad(double p_y) {
+	return p_y * (PI / 180.0);
+}
+_ALWAYS_INLINE_ float deg_to_rad(float p_y) {
+	return p_y * ((float)PI / 180.0f);
+}
+
+_ALWAYS_INLINE_ double rad_to_deg(double p_y) {
+	return p_y * (180.0 / PI);
+}
+_ALWAYS_INLINE_ float rad_to_deg(float p_y) {
+	return p_y * (180.0f / (float)PI);
+}
+
+_ALWAYS_INLINE_ double lerp(double p_from, double p_to, double p_weight) {
+	return p_from + (p_to - p_from) * p_weight;
+}
+_ALWAYS_INLINE_ float lerp(float p_from, float p_to, float p_weight) {
+	return p_from + (p_to - p_from) * p_weight;
+}
+
+_ALWAYS_INLINE_ double cubic_interpolate(double p_from, double p_to, double p_pre, double p_post, double p_weight) {
+	return 0.5 *
+			((p_from * 2.0) +
+					(-p_pre + p_to) * p_weight +
+					(2.0 * p_pre - 5.0 * p_from + 4.0 * p_to - p_post) * (p_weight * p_weight) +
+					(-p_pre + 3.0 * p_from - 3.0 * p_to + p_post) * (p_weight * p_weight * p_weight));
+}
+_ALWAYS_INLINE_ float cubic_interpolate(float p_from, float p_to, float p_pre, float p_post, float p_weight) {
+	return 0.5f *
+			((p_from * 2.0f) +
+					(-p_pre + p_to) * p_weight +
+					(2.0f * p_pre - 5.0f * p_from + 4.0f * p_to - p_post) * (p_weight * p_weight) +
+					(-p_pre + 3.0f * p_from - 3.0f * p_to + p_post) * (p_weight * p_weight * p_weight));
+}
+
+_ALWAYS_INLINE_ double cubic_interpolate_angle(double p_from, double p_to, double p_pre, double p_post, double p_weight) {
+	double from_rot = fmod(p_from, TAU);
+
+	double pre_diff = fmod(p_pre - from_rot, TAU);
+	double pre_rot = from_rot + fmod(2.0 * pre_diff, TAU) - pre_diff;
+
+	double to_diff = fmod(p_to - from_rot, TAU);
+	double to_rot = from_rot + fmod(2.0 * to_diff, TAU) - to_diff;
+
+	double post_diff = fmod(p_post - to_rot, TAU);
+	double post_rot = to_rot + fmod(2.0 * post_diff, TAU) - post_diff;
+
+	return cubic_interpolate(from_rot, to_rot, pre_rot, post_rot, p_weight);
+}
+
+_ALWAYS_INLINE_ float cubic_interpolate_angle(float p_from, float p_to, float p_pre, float p_post, float p_weight) {
+	float from_rot = fmod(p_from, (float)TAU);
+
+	float pre_diff = fmod(p_pre - from_rot, (float)TAU);
+	float pre_rot = from_rot + fmod(2.0f * pre_diff, (float)TAU) - pre_diff;
+
+	float to_diff = fmod(p_to - from_rot, (float)TAU);
+	float to_rot = from_rot + fmod(2.0f * to_diff, (float)TAU) - to_diff;
+
+	float post_diff = fmod(p_post - to_rot, (float)TAU);
+	float post_rot = to_rot + fmod(2.0f * post_diff, (float)TAU) - post_diff;
+
+	return cubic_interpolate(from_rot, to_rot, pre_rot, post_rot, p_weight);
+}
+
+_ALWAYS_INLINE_ double cubic_interpolate_in_time(double p_from, double p_to, double p_pre, double p_post, double p_weight,
+		double p_to_t, double p_pre_t, double p_post_t) {
+	/* Barry-Goldman method */
+	double t = lerp(0.0, p_to_t, p_weight);
+	double a1 = lerp(p_pre, p_from, p_pre_t == 0 ? 0.0 : (t - p_pre_t) / -p_pre_t);
+	double a2 = lerp(p_from, p_to, p_to_t == 0 ? 0.5 : t / p_to_t);
+	double a3 = lerp(p_to, p_post, p_post_t - p_to_t == 0 ? 1.0 : (t - p_to_t) / (p_post_t - p_to_t));
+	double b1 = lerp(a1, a2, p_to_t - p_pre_t == 0 ? 0.0 : (t - p_pre_t) / (p_to_t - p_pre_t));
+	double b2 = lerp(a2, a3, p_post_t == 0 ? 1.0 : t / p_post_t);
+	return lerp(b1, b2, p_to_t == 0 ? 0.5 : t / p_to_t);
+}
+_ALWAYS_INLINE_ float cubic_interpolate_in_time(float p_from, float p_to, float p_pre, float p_post, float p_weight,
+		float p_to_t, float p_pre_t, float p_post_t) {
+	/* Barry-Goldman method */
+	float t = lerp(0.0f, p_to_t, p_weight);
+	float a1 = lerp(p_pre, p_from, p_pre_t == 0 ? 0.0f : (t - p_pre_t) / -p_pre_t);
+	float a2 = lerp(p_from, p_to, p_to_t == 0 ? 0.5f : t / p_to_t);
+	float a3 = lerp(p_to, p_post, p_post_t - p_to_t == 0 ? 1.0f : (t - p_to_t) / (p_post_t - p_to_t));
+	float b1 = lerp(a1, a2, p_to_t - p_pre_t == 0 ? 0.0f : (t - p_pre_t) / (p_to_t - p_pre_t));
+	float b2 = lerp(a2, a3, p_post_t == 0 ? 1.0f : t / p_post_t);
+	return lerp(b1, b2, p_to_t == 0 ? 0.5f : t / p_to_t);
+}
+
+_ALWAYS_INLINE_ double cubic_interpolate_angle_in_time(double p_from, double p_to, double p_pre, double p_post, double p_weight,
+		double p_to_t, double p_pre_t, double p_post_t) {
+	double from_rot = fmod(p_from, TAU);
+
+	double pre_diff = fmod(p_pre - from_rot, TAU);
+	double pre_rot = from_rot + fmod(2.0 * pre_diff, TAU) - pre_diff;
+
+	double to_diff = fmod(p_to - from_rot, TAU);
+	double to_rot = from_rot + fmod(2.0 * to_diff, TAU) - to_diff;
+
+	double post_diff = fmod(p_post - to_rot, TAU);
+	double post_rot = to_rot + fmod(2.0 * post_diff, TAU) - post_diff;
+
+	return cubic_interpolate_in_time(from_rot, to_rot, pre_rot, post_rot, p_weight, p_to_t, p_pre_t, p_post_t);
+}
+_ALWAYS_INLINE_ float cubic_interpolate_angle_in_time(float p_from, float p_to, float p_pre, float p_post, float p_weight,
+		float p_to_t, float p_pre_t, float p_post_t) {
+	float from_rot = fmod(p_from, (float)TAU);
+
+	float pre_diff = fmod(p_pre - from_rot, (float)TAU);
+	float pre_rot = from_rot + fmod(2.0f * pre_diff, (float)TAU) - pre_diff;
+
+	float to_diff = fmod(p_to - from_rot, (float)TAU);
+	float to_rot = from_rot + fmod(2.0f * to_diff, (float)TAU) - to_diff;
+
+	float post_diff = fmod(p_post - to_rot, (float)TAU);
+	float post_rot = to_rot + fmod(2.0f * post_diff, (float)TAU) - post_diff;
+
+	return cubic_interpolate_in_time(from_rot, to_rot, pre_rot, post_rot, p_weight, p_to_t, p_pre_t, p_post_t);
+}
+
+_ALWAYS_INLINE_ double bezier_interpolate(double p_start, double p_control_1, double p_control_2, double p_end, double p_t) {
+	/* Formula from Wikipedia article on Bezier curves. */
+	double omt = (1.0 - p_t);
+	double omt2 = omt * omt;
+	double omt3 = omt2 * omt;
+	double t2 = p_t * p_t;
+	double t3 = t2 * p_t;
+
+	return p_start * omt3 + p_control_1 * omt2 * p_t * 3.0 + p_control_2 * omt * t2 * 3.0 + p_end * t3;
+}
+_ALWAYS_INLINE_ float bezier_interpolate(float p_start, float p_control_1, float p_control_2, float p_end, float p_t) {
+	/* Formula from Wikipedia article on Bezier curves. */
+	float omt = (1.0f - p_t);
+	float omt2 = omt * omt;
+	float omt3 = omt2 * omt;
+	float t2 = p_t * p_t;
+	float t3 = t2 * p_t;
+
+	return p_start * omt3 + p_control_1 * omt2 * p_t * 3.0f + p_control_2 * omt * t2 * 3.0f + p_end * t3;
+}
+
+_ALWAYS_INLINE_ double bezier_derivative(double p_start, double p_control_1, double p_control_2, double p_end, double p_t) {
+	/* Formula from Wikipedia article on Bezier curves. */
+	double omt = (1.0 - p_t);
+	double omt2 = omt * omt;
+	double t2 = p_t * p_t;
+
+	double d = (p_control_1 - p_start) * 3.0 * omt2 + (p_control_2 - p_control_1) * 6.0 * omt * p_t + (p_end - p_control_2) * 3.0 * t2;
+	return d;
+}
+_ALWAYS_INLINE_ float bezier_derivative(float p_start, float p_control_1, float p_control_2, float p_end, float p_t) {
+	/* Formula from Wikipedia article on Bezier curves. */
+	float omt = (1.0f - p_t);
+	float omt2 = omt * omt;
+	float t2 = p_t * p_t;
+
+	float d = (p_control_1 - p_start) * 3.0f * omt2 + (p_control_2 - p_control_1) * 6.0f * omt * p_t + (p_end - p_control_2) * 3.0f * t2;
+	return d;
+}
+
+_ALWAYS_INLINE_ double angle_difference(double p_from, double p_to) {
+	double difference = fmod(p_to - p_from, TAU);
+	return fmod(2.0 * difference, TAU) - difference;
+}
+_ALWAYS_INLINE_ float angle_difference(float p_from, float p_to) {
+	float difference = fmod(p_to - p_from, (float)TAU);
+	return fmod(2.0f * difference, (float)TAU) - difference;
+}
+
+_ALWAYS_INLINE_ double lerp_angle(double p_from, double p_to, double p_weight) {
+	return p_from + angle_difference(p_from, p_to) * p_weight;
+}
+_ALWAYS_INLINE_ float lerp_angle(float p_from, float p_to, float p_weight) {
+	return p_from + angle_difference(p_from, p_to) * p_weight;
+}
+
+_ALWAYS_INLINE_ double inverse_lerp(double p_from, double p_to, double p_value) {
+	return (p_value - p_from) / (p_to - p_from);
+}
+_ALWAYS_INLINE_ float inverse_lerp(float p_from, float p_to, float p_value) {
+	return (p_value - p_from) / (p_to - p_from);
+}
+
+_ALWAYS_INLINE_ double remap(double p_value, double p_istart, double p_istop, double p_ostart, double p_ostop) {
+	return lerp(p_ostart, p_ostop, inverse_lerp(p_istart, p_istop, p_value));
+}
+_ALWAYS_INLINE_ float remap(float p_value, float p_istart, float p_istop, float p_ostart, float p_ostop) {
+	return lerp(p_ostart, p_ostop, inverse_lerp(p_istart, p_istop, p_value));
+}
+
+_ALWAYS_INLINE_ bool is_equal_approx(double p_left, double p_right, double p_tolerance) {
+	// Check for exact equality first, required to handle "infinity" values.
+	if (p_left == p_right) {
+		return true;
+	}
+	// Then check for approximate equality.
+	return abs(p_left - p_right) < p_tolerance;
+}
+_ALWAYS_INLINE_ bool is_equal_approx(float p_left, float p_right, float p_tolerance) {
+	// Check for exact equality first, required to handle "infinity" values.
+	if (p_left == p_right) {
+		return true;
+	}
+	// Then check for approximate equality.
+	return abs(p_left - p_right) < p_tolerance;
+}
+
+_ALWAYS_INLINE_ bool is_equal_approx(double p_left, double p_right) {
+	// Check for exact equality first, required to handle "infinity" values.
+	if (p_left == p_right) {
+		return true;
+	}
+	// Then check for approximate equality.
+	double tolerance = CMP_EPSILON * abs(p_left);
+	if (tolerance < CMP_EPSILON) {
+		tolerance = CMP_EPSILON;
+	}
+	return abs(p_left - p_right) < tolerance;
+}
+_ALWAYS_INLINE_ bool is_equal_approx(float p_left, float p_right) {
+	// Check for exact equality first, required to handle "infinity" values.
+	if (p_left == p_right) {
+		return true;
+	}
+	// Then check for approximate equality.
+	float tolerance = (float)CMP_EPSILON * abs(p_left);
+	if (tolerance < (float)CMP_EPSILON) {
+		tolerance = (float)CMP_EPSILON;
+	}
+	return abs(p_left - p_right) < tolerance;
+}
+
+_ALWAYS_INLINE_ bool is_zero_approx(double p_value) {
+	return abs(p_value) < CMP_EPSILON;
+}
+_ALWAYS_INLINE_ bool is_zero_approx(float p_value) {
+	return abs(p_value) < (float)CMP_EPSILON;
+}
+
+_ALWAYS_INLINE_ bool is_same(double p_left, double p_right) {
+	return (p_left == p_right) || (is_nan(p_left) && is_nan(p_right));
+}
+_ALWAYS_INLINE_ bool is_same(float p_left, float p_right) {
+	return (p_left == p_right) || (is_nan(p_left) && is_nan(p_right));
+}
+
+_ALWAYS_INLINE_ double smoothstep(double p_from, double p_to, double p_s) {
+	if (is_equal_approx(p_from, p_to)) {
+		if (likely(p_from <= p_to)) {
+			return p_s <= p_from ? 0.0 : 1.0;
+		} else {
+			return p_s <= p_to ? 1.0 : 0.0;
+		}
+	}
+	double s = CLAMP((p_s - p_from) / (p_to - p_from), 0.0, 1.0);
+	return s * s * (3.0 - 2.0 * s);
+}
+_ALWAYS_INLINE_ float smoothstep(float p_from, float p_to, float p_s) {
+	if (is_equal_approx(p_from, p_to)) {
+		if (likely(p_from <= p_to)) {
+			return p_s <= p_from ? 0.0f : 1.0f;
+		} else {
+			return p_s <= p_to ? 1.0f : 0.0f;
+		}
+	}
+	float s = CLAMP((p_s - p_from) / (p_to - p_from), 0.0f, 1.0f);
+	return s * s * (3.0f - 2.0f * s);
+}
+
+_ALWAYS_INLINE_ double move_toward(double p_from, double p_to, double p_delta) {
+	return abs(p_to - p_from) <= p_delta ? p_to : p_from + SIGN(p_to - p_from) * p_delta;
+}
+_ALWAYS_INLINE_ float move_toward(float p_from, float p_to, float p_delta) {
+	return abs(p_to - p_from) <= p_delta ? p_to : p_from + SIGN(p_to - p_from) * p_delta;
+}
+
+_ALWAYS_INLINE_ double rotate_toward(double p_from, double p_to, double p_delta) {
+	double difference = angle_difference(p_from, p_to);
+	double abs_difference = abs(difference);
+	// When `p_delta < 0` move no further than to PI radians away from `p_to` (as PI is the max possible angle distance).
+	return p_from + CLAMP(p_delta, abs_difference - PI, abs_difference) * (difference >= 0.0 ? 1.0 : -1.0);
+}
+_ALWAYS_INLINE_ float rotate_toward(float p_from, float p_to, float p_delta) {
+	float difference = angle_difference(p_from, p_to);
+	float abs_difference = abs(difference);
+	// When `p_delta < 0` move no further than to PI radians away from `p_to` (as PI is the max possible angle distance).
+	return p_from + CLAMP(p_delta, abs_difference - (float)PI, abs_difference) * (difference >= 0.0f ? 1.0f : -1.0f);
+}
+
+_ALWAYS_INLINE_ double linear_to_db(double p_linear) {
+	return log(p_linear) * 8.6858896380650365530225783783321;
+}
+_ALWAYS_INLINE_ float linear_to_db(float p_linear) {
+	return log(p_linear) * (float)8.6858896380650365530225783783321;
+}
+
+_ALWAYS_INLINE_ double db_to_linear(double p_db) {
+	return exp(p_db * 0.11512925464970228420089957273422);
+}
+_ALWAYS_INLINE_ float db_to_linear(float p_db) {
+	return exp(p_db * (float)0.11512925464970228420089957273422);
+}
+
+_ALWAYS_INLINE_ double round(double p_val) {
+	return std::round(p_val);
+}
+_ALWAYS_INLINE_ float round(float p_val) {
+	return std::round(p_val);
+}
+
+_ALWAYS_INLINE_ double wrapf(double p_value, double p_min, double p_max) {
+	double range = p_max - p_min;
+	if (is_zero_approx(range)) {
+		return p_min;
+	}
+	double result = p_value - (range * floor((p_value - p_min) / range));
+	if (is_equal_approx(result, p_max)) {
+		return p_min;
+	}
+	return result;
+}
+_ALWAYS_INLINE_ float wrapf(float p_value, float p_min, float p_max) {
+	float range = p_max - p_min;
+	if (is_zero_approx(range)) {
+		return p_min;
+	}
+	float result = p_value - (range * floor((p_value - p_min) / range));
+	if (is_equal_approx(result, p_max)) {
+		return p_min;
+	}
+	return result;
+}
+
+_ALWAYS_INLINE_ int64_t wrapi(int64_t p_value, int64_t p_min, int64_t p_max) {
+	int64_t range = p_max - p_min;
+	return range == 0 ? p_min : p_min + ((((p_value - p_min) % range) + range) % range);
+}
+
+_ALWAYS_INLINE_ double fract(double p_value) {
+	return p_value - floor(p_value);
+}
+_ALWAYS_INLINE_ float fract(float p_value) {
+	return p_value - floor(p_value);
+}
+
+_ALWAYS_INLINE_ double pingpong(double p_value, double p_length) {
+	return (p_length != 0.0) ? abs(fract((p_value - p_length) / (p_length * 2.0)) * p_length * 2.0 - p_length) : 0.0;
+}
+_ALWAYS_INLINE_ float pingpong(float p_value, float p_length) {
+	return (p_length != 0.0f) ? abs(fract((p_value - p_length) / (p_length * 2.0f)) * p_length * 2.0f - p_length) : 0.0f;
+}
+
+// double only, as these functions are mainly used by the editor and not performance-critical,
+double ease(double p_x, double p_c);
+int step_decimals(double p_step);
+int range_step_decimals(double p_step); // For editor use only.
+double snapped(double p_value, double p_step);
+
+uint32_t larger_prime(uint32_t p_val);
+
+void seed(uint64_t p_seed);
+void randomize();
+uint32_t rand_from_seed(uint64_t *p_seed);
+uint32_t rand();
+_ALWAYS_INLINE_ double randd() {
+	return (double)rand() / (double)UINT32_MAX;
+}
+_ALWAYS_INLINE_ float randf() {
+	return (float)rand() / (float)UINT32_MAX;
+}
+double randfn(double p_mean, double p_deviation);
+
+double random(double p_from, double p_to);
+float random(float p_from, float p_to);
+int random(int p_from, int p_to);
+
+// This function should be as fast as possible and rounding mode should not matter.
+_ALWAYS_INLINE_ int fast_ftoi(float p_value) {
+	return std::rint(p_value);
+}
+
+_ALWAYS_INLINE_ uint32_t halfbits_to_floatbits(uint16_t p_half) {
+	uint16_t h_exp, h_sig;
+	uint32_t f_sgn, f_exp, f_sig;
+
+	h_exp = (p_half & 0x7c00u);
+	f_sgn = ((uint32_t)p_half & 0x8000u) << 16;
+	switch (h_exp) {
+		case 0x0000u: /* 0 or subnormal */
+			h_sig = (p_half & 0x03ffu);
+			/* Signed zero */
+			if (h_sig == 0) {
+				return f_sgn;
+			}
+			/* Subnormal */
+			h_sig <<= 1;
+			while ((h_sig & 0x0400u) == 0) {
+				h_sig <<= 1;
+				h_exp++;
+			}
+			f_exp = ((uint32_t)(127 - 15 - h_exp)) << 23;
+			f_sig = ((uint32_t)(h_sig & 0x03ffu)) << 13;
+			return f_sgn + f_exp + f_sig;
+		case 0x7c00u: /* inf or NaN */
+			/* All-ones exponent and a copy of the significand */
+			return f_sgn + 0x7f800000u + (((uint32_t)(p_half & 0x03ffu)) << 13);
+		default: /* normalized */
+			/* Just need to adjust the exponent and shift */
+			return f_sgn + (((uint32_t)(p_half & 0x7fffu) + 0x1c000u) << 13);
+	}
+}
+
+_ALWAYS_INLINE_ float halfptr_to_float(const uint16_t *p_half) {
+	union {
+		uint32_t u32;
+		float f32;
+	} u;
+
+	u.u32 = halfbits_to_floatbits(*p_half);
+	return u.f32;
+}
+
+_ALWAYS_INLINE_ float half_to_float(const uint16_t p_half) {
+	return halfptr_to_float(&p_half);
+}
+
+_ALWAYS_INLINE_ uint16_t make_half_float(float p_value) {
+	union {
+		float fv;
+		uint32_t ui;
+	} ci;
+	ci.fv = p_value;
+
+	uint32_t x = ci.ui;
+	uint32_t sign = (unsigned short)(x >> 31);
+	uint32_t mantissa;
+	uint32_t exponent;
+	uint16_t hf;
+
+	// get mantissa
+	mantissa = x & ((1 << 23) - 1);
+	// get exponent bits
+	exponent = x & (0xFF << 23);
+	if (exponent >= 0x47800000) {
+		// check if the original single precision float number is a NaN
+		if (mantissa && (exponent == (0xFF << 23))) {
+			// we have a single precision NaN
+			mantissa = (1 << 23) - 1;
+		} else {
+			// 16-bit half-float representation stores number as Inf
+			mantissa = 0;
+		}
+		hf = (((uint16_t)sign) << 15) | (uint16_t)((0x1F << 10)) |
+				(uint16_t)(mantissa >> 13);
+	}
+	// check if exponent is <= -15
+	else if (exponent <= 0x38000000) {
+		/*
+		// store a denorm half-float value or zero
+		exponent = (0x38000000 - exponent) >> 23;
+		mantissa >>= (14 + exponent);
+
+		hf = (((uint16_t)sign) << 15) | (uint16_t)(mantissa);
+		*/
+		hf = 0; //denormals do not work for 3D, convert to zero
+	} else {
+		hf = (((uint16_t)sign) << 15) |
+				(uint16_t)((exponent - 0x38000000) >> 13) |
+				(uint16_t)(mantissa >> 13);
+	}
+
+	return hf;
+}
+
+_ALWAYS_INLINE_ float snap_scalar(float p_offset, float p_step, float p_target) {
+	return p_step != 0 ? snapped(p_target - p_offset, p_step) + p_offset : p_target;
+}
+
+_ALWAYS_INLINE_ float snap_scalar_separation(float p_offset, float p_step, float p_target, float p_separation) {
+	if (p_step != 0) {
+		float a = snapped(p_target - p_offset, p_step + p_separation) + p_offset;
+		float b = a;
+		if (p_target >= 0) {
+			b -= p_separation;
+		} else {
+			b += p_step;
+		}
+		return (abs(p_target - a) < abs(p_target - b)) ? a : b;
+	}
+	return p_target;
+}
+
+}; // namespace Math

core/math/plane.cpp

@@ -0,0 +1,185 @@
+/**************************************************************************/
+/*  plane.cpp                                                             */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "plane.h"
+
+#include "core/math/math_funcs.h"
+#include "core/variant/variant.h"
+
+void Plane::set_normal(const Vector3 &p_normal) {
+	normal = p_normal;
+}
+
+void Plane::normalize() {
+	real_t l = normal.length();
+	if (l == 0) {
+		*this = Plane(0, 0, 0, 0);
+		return;
+	}
+	normal /= l;
+	d /= l;
+}
+
+Plane Plane::normalized() const {
+	Plane p = *this;
+	p.normalize();
+	return p;
+}
+
+Vector3 Plane::get_any_perpendicular_normal() const {
+	static const Vector3 p1 = Vector3(1, 0, 0);
+	static const Vector3 p2 = Vector3(0, 1, 0);
+	Vector3 p;
+
+	if (Math::abs(normal.dot(p1)) > 0.99f) { // if too similar to p1
+		p = p2; // use p2
+	} else {
+		p = p1; // use p1
+	}
+
+	p -= normal * normal.dot(p);
+	p.normalize();
+
+	return p;
+}
+
+/* intersections */
+
+bool Plane::intersect_3(const Plane &p_plane1, const Plane &p_plane2, Vector3 *r_result) const {
+	const Plane &p_plane0 = *this;
+	Vector3 normal0 = p_plane0.normal;
+	Vector3 normal1 = p_plane1.normal;
+	Vector3 normal2 = p_plane2.normal;
+
+	real_t denom = vec3_cross(normal0, normal1).dot(normal2);
+
+	if (Math::is_zero_approx(denom)) {
+		return false;
+	}
+
+	if (r_result) {
+		*r_result = ((vec3_cross(normal1, normal2) * p_plane0.d) +
+							(vec3_cross(normal2, normal0) * p_plane1.d) +
+							(vec3_cross(normal0, normal1) * p_plane2.d)) /
+				denom;
+	}
+
+	return true;
+}
+
+bool Plane::intersects_ray(const Vector3 &p_from, const Vector3 &p_dir, Vector3 *p_intersection) const {
+	Vector3 segment = p_dir;
+	real_t den = normal.dot(segment);
+
+	if (Math::is_zero_approx(den)) {
+		return false;
+	}
+
+	real_t dist = (normal.dot(p_from) - d) / den;
+
+	if (dist > (real_t)CMP_EPSILON) { //this is a ray, before the emitting pos (p_from) doesn't exist
+
+		return false;
+	}
+
+	dist = -dist;
+	*p_intersection = p_from + segment * dist;
+
+	return true;
+}
+
+bool Plane::intersects_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 *p_intersection) const {
+	Vector3 segment = p_begin - p_end;
+	real_t den = normal.dot(segment);
+
+	if (Math::is_zero_approx(den)) {
+		return false;
+	}
+
+	real_t dist = (normal.dot(p_begin) - d) / den;
+
+	if (dist < (real_t)-CMP_EPSILON || dist > (1.0f + (real_t)CMP_EPSILON)) {
+		return false;
+	}
+
+	dist = -dist;
+	*p_intersection = p_begin + segment * dist;
+
+	return true;
+}
+
+Variant Plane::intersect_3_bind(const Plane &p_plane1, const Plane &p_plane2) const {
+	Vector3 inters;
+	if (intersect_3(p_plane1, p_plane2, &inters)) {
+		return inters;
+	} else {
+		return Variant();
+	}
+}
+
+Variant Plane::intersects_ray_bind(const Vector3 &p_from, const Vector3 &p_dir) const {
+	Vector3 inters;
+	if (intersects_ray(p_from, p_dir, &inters)) {
+		return inters;
+	} else {
+		return Variant();
+	}
+}
+
+Variant Plane::intersects_segment_bind(const Vector3 &p_begin, const Vector3 &p_end) const {
+	Vector3 inters;
+	if (intersects_segment(p_begin, p_end, &inters)) {
+		return inters;
+	} else {
+		return Variant();
+	}
+}
+
+/* misc */
+
+bool Plane::is_equal_approx_any_side(const Plane &p_plane) const {
+	return (normal.is_equal_approx(p_plane.normal) && Math::is_equal_approx(d, p_plane.d)) || (normal.is_equal_approx(-p_plane.normal) && Math::is_equal_approx(d, -p_plane.d));
+}
+
+bool Plane::is_equal_approx(const Plane &p_plane) const {
+	return normal.is_equal_approx(p_plane.normal) && Math::is_equal_approx(d, p_plane.d);
+}
+
+bool Plane::is_same(const Plane &p_plane) const {
+	return normal.is_same(p_plane.normal) && Math::is_same(d, p_plane.d);
+}
+
+bool Plane::is_finite() const {
+	return normal.is_finite() && Math::is_finite(d);
+}
+
+Plane::operator String() const {
+	return "[N: " + normal.operator String() + ", D: " + String::num_real(d, false) + "]";
+}

core/math/plane.h

@@ -0,0 +1,137 @@
+/**************************************************************************/
+/*  plane.h                                                               */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "core/math/vector3.h"
+
+class Variant;
+
+struct [[nodiscard]] Plane {
+	Vector3 normal;
+	real_t d = 0;
+
+	void set_normal(const Vector3 &p_normal);
+	_FORCE_INLINE_ Vector3 get_normal() const { return normal; }
+
+	void normalize();
+	Plane normalized() const;
+
+	/* Plane-Point operations */
+
+	_FORCE_INLINE_ Vector3 get_center() const { return normal * d; }
+	Vector3 get_any_perpendicular_normal() const;
+
+	_FORCE_INLINE_ bool is_point_over(const Vector3 &p_point) const; ///< Point is over plane
+	_FORCE_INLINE_ real_t distance_to(const Vector3 &p_point) const;
+	_FORCE_INLINE_ bool has_point(const Vector3 &p_point, real_t p_tolerance = CMP_EPSILON) const;
+
+	/* intersections */
+
+	bool intersect_3(const Plane &p_plane1, const Plane &p_plane2, Vector3 *r_result = nullptr) const;
+	bool intersects_ray(const Vector3 &p_from, const Vector3 &p_dir, Vector3 *p_intersection) const;
+	bool intersects_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 *p_intersection) const;
+
+	// For Variant bindings.
+	Variant intersect_3_bind(const Plane &p_plane1, const Plane &p_plane2) const;
+	Variant intersects_ray_bind(const Vector3 &p_from, const Vector3 &p_dir) const;
+	Variant intersects_segment_bind(const Vector3 &p_begin, const Vector3 &p_end) const;
+
+	_FORCE_INLINE_ Vector3 project(const Vector3 &p_point) const {
+		return p_point - normal * distance_to(p_point);
+	}
+
+	/* misc */
+
+	constexpr Plane operator-() const { return Plane(-normal, -d); }
+	bool is_equal_approx(const Plane &p_plane) const;
+	bool is_same(const Plane &p_plane) const;
+	bool is_equal_approx_any_side(const Plane &p_plane) const;
+	bool is_finite() const;
+
+	constexpr bool operator==(const Plane &p_plane) const;
+	constexpr bool operator!=(const Plane &p_plane) const;
+	explicit operator String() const;
+
+	Plane() = default;
+	constexpr Plane(real_t p_a, real_t p_b, real_t p_c, real_t p_d) :
+			normal(p_a, p_b, p_c),
+			d(p_d) {}
+
+	constexpr Plane(const Vector3 &p_normal, real_t p_d = 0.0);
+	_FORCE_INLINE_ Plane(const Vector3 &p_normal, const Vector3 &p_point);
+	_FORCE_INLINE_ Plane(const Vector3 &p_point1, const Vector3 &p_point2, const Vector3 &p_point3, ClockDirection p_dir = CLOCKWISE);
+};
+
+bool Plane::is_point_over(const Vector3 &p_point) const {
+	return (normal.dot(p_point) > d);
+}
+
+real_t Plane::distance_to(const Vector3 &p_point) const {
+	return (normal.dot(p_point) - d);
+}
+
+bool Plane::has_point(const Vector3 &p_point, real_t p_tolerance) const {
+	real_t dist = normal.dot(p_point) - d;
+	dist = Math::abs(dist);
+	return (dist <= p_tolerance);
+}
+
+constexpr Plane::Plane(const Vector3 &p_normal, real_t p_d) :
+		normal(p_normal),
+		d(p_d) {
+}
+
+Plane::Plane(const Vector3 &p_normal, const Vector3 &p_point) :
+		normal(p_normal),
+		d(p_normal.dot(p_point)) {
+}
+
+Plane::Plane(const Vector3 &p_point1, const Vector3 &p_point2, const Vector3 &p_point3, ClockDirection p_dir) {
+	if (p_dir == CLOCKWISE) {
+		normal = (p_point1 - p_point3).cross(p_point1 - p_point2);
+	} else {
+		normal = (p_point1 - p_point2).cross(p_point1 - p_point3);
+	}
+
+	normal.normalize();
+	d = normal.dot(p_point1);
+}
+
+constexpr bool Plane::operator==(const Plane &p_plane) const {
+	return normal == p_plane.normal && d == p_plane.d;
+}
+
+constexpr bool Plane::operator!=(const Plane &p_plane) const {
+	return normal != p_plane.normal || d != p_plane.d;
+}
+
+template <>
+struct is_zero_constructible<Plane> : std::true_type {};

core/math/projection.cpp

@@ -0,0 +1,980 @@
+/**************************************************************************/
+/*  projection.cpp                                                        */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "projection.h"
+
+#include "core/math/aabb.h"
+#include "core/math/math_funcs.h"
+#include "core/math/plane.h"
+#include "core/math/rect2.h"
+#include "core/math/transform_3d.h"
+#include "core/string/ustring.h"
+
+real_t Projection::determinant() const {
+	return columns[0][3] * columns[1][2] * columns[2][1] * columns[3][0] - columns[0][2] * columns[1][3] * columns[2][1] * columns[3][0] -
+			columns[0][3] * columns[1][1] * columns[2][2] * columns[3][0] + columns[0][1] * columns[1][3] * columns[2][2] * columns[3][0] +
+			columns[0][2] * columns[1][1] * columns[2][3] * columns[3][0] - columns[0][1] * columns[1][2] * columns[2][3] * columns[3][0] -
+			columns[0][3] * columns[1][2] * columns[2][0] * columns[3][1] + columns[0][2] * columns[1][3] * columns[2][0] * columns[3][1] +
+			columns[0][3] * columns[1][0] * columns[2][2] * columns[3][1] - columns[0][0] * columns[1][3] * columns[2][2] * columns[3][1] -
+			columns[0][2] * columns[1][0] * columns[2][3] * columns[3][1] + columns[0][0] * columns[1][2] * columns[2][3] * columns[3][1] +
+			columns[0][3] * columns[1][1] * columns[2][0] * columns[3][2] - columns[0][1] * columns[1][3] * columns[2][0] * columns[3][2] -
+			columns[0][3] * columns[1][0] * columns[2][1] * columns[3][2] + columns[0][0] * columns[1][3] * columns[2][1] * columns[3][2] +
+			columns[0][1] * columns[1][0] * columns[2][3] * columns[3][2] - columns[0][0] * columns[1][1] * columns[2][3] * columns[3][2] -
+			columns[0][2] * columns[1][1] * columns[2][0] * columns[3][3] + columns[0][1] * columns[1][2] * columns[2][0] * columns[3][3] +
+			columns[0][2] * columns[1][0] * columns[2][1] * columns[3][3] - columns[0][0] * columns[1][2] * columns[2][1] * columns[3][3] -
+			columns[0][1] * columns[1][0] * columns[2][2] * columns[3][3] + columns[0][0] * columns[1][1] * columns[2][2] * columns[3][3];
+}
+
+void Projection::set_identity() {
+	for (int i = 0; i < 4; i++) {
+		for (int j = 0; j < 4; j++) {
+			columns[i][j] = (i == j) ? 1 : 0;
+		}
+	}
+}
+
+void Projection::set_zero() {
+	for (int i = 0; i < 4; i++) {
+		for (int j = 0; j < 4; j++) {
+			columns[i][j] = 0;
+		}
+	}
+}
+
+Plane Projection::xform4(const Plane &p_vec4) const {
+	Plane ret;
+
+	ret.normal.x = columns[0][0] * p_vec4.normal.x + columns[1][0] * p_vec4.normal.y + columns[2][0] * p_vec4.normal.z + columns[3][0] * p_vec4.d;
+	ret.normal.y = columns[0][1] * p_vec4.normal.x + columns[1][1] * p_vec4.normal.y + columns[2][1] * p_vec4.normal.z + columns[3][1] * p_vec4.d;
+	ret.normal.z = columns[0][2] * p_vec4.normal.x + columns[1][2] * p_vec4.normal.y + columns[2][2] * p_vec4.normal.z + columns[3][2] * p_vec4.d;
+	ret.d = columns[0][3] * p_vec4.normal.x + columns[1][3] * p_vec4.normal.y + columns[2][3] * p_vec4.normal.z + columns[3][3] * p_vec4.d;
+	return ret;
+}
+
+Vector4 Projection::xform(const Vector4 &p_vec4) const {
+	return Vector4(
+			columns[0][0] * p_vec4.x + columns[1][0] * p_vec4.y + columns[2][0] * p_vec4.z + columns[3][0] * p_vec4.w,
+			columns[0][1] * p_vec4.x + columns[1][1] * p_vec4.y + columns[2][1] * p_vec4.z + columns[3][1] * p_vec4.w,
+			columns[0][2] * p_vec4.x + columns[1][2] * p_vec4.y + columns[2][2] * p_vec4.z + columns[3][2] * p_vec4.w,
+			columns[0][3] * p_vec4.x + columns[1][3] * p_vec4.y + columns[2][3] * p_vec4.z + columns[3][3] * p_vec4.w);
+}
+Vector4 Projection::xform_inv(const Vector4 &p_vec4) const {
+	return Vector4(
+			columns[0][0] * p_vec4.x + columns[0][1] * p_vec4.y + columns[0][2] * p_vec4.z + columns[0][3] * p_vec4.w,
+			columns[1][0] * p_vec4.x + columns[1][1] * p_vec4.y + columns[1][2] * p_vec4.z + columns[1][3] * p_vec4.w,
+			columns[2][0] * p_vec4.x + columns[2][1] * p_vec4.y + columns[2][2] * p_vec4.z + columns[2][3] * p_vec4.w,
+			columns[3][0] * p_vec4.x + columns[3][1] * p_vec4.y + columns[3][2] * p_vec4.z + columns[3][3] * p_vec4.w);
+}
+
+void Projection::adjust_perspective_znear(real_t p_new_znear) {
+	real_t zfar = get_z_far();
+	real_t znear = p_new_znear;
+
+	real_t deltaZ = zfar - znear;
+	columns[2][2] = -(zfar + znear) / deltaZ;
+	columns[3][2] = -2 * znear * zfar / deltaZ;
+}
+
+Projection Projection::create_depth_correction(bool p_flip_y) {
+	Projection proj;
+	proj.set_depth_correction(p_flip_y);
+	return proj;
+}
+
+Projection Projection::create_light_atlas_rect(const Rect2 &p_rect) {
+	Projection proj;
+	proj.set_light_atlas_rect(p_rect);
+	return proj;
+}
+
+Projection Projection::create_perspective(real_t p_fovy_degrees, real_t p_aspect, real_t p_z_near, real_t p_z_far, bool p_flip_fov) {
+	Projection proj;
+	proj.set_perspective(p_fovy_degrees, p_aspect, p_z_near, p_z_far, p_flip_fov);
+	return proj;
+}
+
+Projection Projection::create_perspective_hmd(real_t p_fovy_degrees, real_t p_aspect, real_t p_z_near, real_t p_z_far, bool p_flip_fov, int p_eye, real_t p_intraocular_dist, real_t p_convergence_dist) {
+	Projection proj;
+	proj.set_perspective(p_fovy_degrees, p_aspect, p_z_near, p_z_far, p_flip_fov, p_eye, p_intraocular_dist, p_convergence_dist);
+	return proj;
+}
+
+Projection Projection::create_for_hmd(int p_eye, real_t p_aspect, real_t p_intraocular_dist, real_t p_display_width, real_t p_display_to_lens, real_t p_oversample, real_t p_z_near, real_t p_z_far) {
+	Projection proj;
+	proj.set_for_hmd(p_eye, p_aspect, p_intraocular_dist, p_display_width, p_display_to_lens, p_oversample, p_z_near, p_z_far);
+	return proj;
+}
+
+Projection Projection::create_orthogonal(real_t p_left, real_t p_right, real_t p_bottom, real_t p_top, real_t p_znear, real_t p_zfar) {
+	Projection proj;
+	proj.set_orthogonal(p_left, p_right, p_bottom, p_top, p_znear, p_zfar);
+	return proj;
+}
+
+Projection Projection::create_orthogonal_aspect(real_t p_size, real_t p_aspect, real_t p_znear, real_t p_zfar, bool p_flip_fov) {
+	Projection proj;
+	proj.set_orthogonal(p_size, p_aspect, p_znear, p_zfar, p_flip_fov);
+	return proj;
+}
+
+Projection Projection::create_frustum(real_t p_left, real_t p_right, real_t p_bottom, real_t p_top, real_t p_near, real_t p_far) {
+	Projection proj;
+	proj.set_frustum(p_left, p_right, p_bottom, p_top, p_near, p_far);
+	return proj;
+}
+
+Projection Projection::create_frustum_aspect(real_t p_size, real_t p_aspect, Vector2 p_offset, real_t p_near, real_t p_far, bool p_flip_fov) {
+	Projection proj;
+	proj.set_frustum(p_size, p_aspect, p_offset, p_near, p_far, p_flip_fov);
+	return proj;
+}
+
+Projection Projection::create_fit_aabb(const AABB &p_aabb) {
+	Projection proj;
+	proj.scale_translate_to_fit(p_aabb);
+	return proj;
+}
+
+Projection Projection::perspective_znear_adjusted(real_t p_new_znear) const {
+	Projection proj = *this;
+	proj.adjust_perspective_znear(p_new_znear);
+	return proj;
+}
+
+Plane Projection::get_projection_plane(Planes p_plane) const {
+	const real_t *matrix = (const real_t *)columns;
+
+	switch (p_plane) {
+		case PLANE_NEAR: {
+			Plane new_plane = Plane(matrix[3] + matrix[2],
+					matrix[7] + matrix[6],
+					matrix[11] + matrix[10],
+					matrix[15] + matrix[14]);
+
+			new_plane.normal = -new_plane.normal;
+			new_plane.normalize();
+			return new_plane;
+		}
+		case PLANE_FAR: {
+			Plane new_plane = Plane(matrix[3] - matrix[2],
+					matrix[7] - matrix[6],
+					matrix[11] - matrix[10],
+					matrix[15] - matrix[14]);
+
+			new_plane.normal = -new_plane.normal;
+			new_plane.normalize();
+			return new_plane;
+		}
+		case PLANE_LEFT: {
+			Plane new_plane = Plane(matrix[3] + matrix[0],
+					matrix[7] + matrix[4],
+					matrix[11] + matrix[8],
+					matrix[15] + matrix[12]);
+
+			new_plane.normal = -new_plane.normal;
+			new_plane.normalize();
+			return new_plane;
+		}
+		case PLANE_TOP: {
+			Plane new_plane = Plane(matrix[3] - matrix[1],
+					matrix[7] - matrix[5],
+					matrix[11] - matrix[9],
+					matrix[15] - matrix[13]);
+
+			new_plane.normal = -new_plane.normal;
+			new_plane.normalize();
+			return new_plane;
+		}
+		case PLANE_RIGHT: {
+			Plane new_plane = Plane(matrix[3] - matrix[0],
+					matrix[7] - matrix[4],
+					matrix[11] - matrix[8],
+					matrix[15] - matrix[12]);
+
+			new_plane.normal = -new_plane.normal;
+			new_plane.normalize();
+			return new_plane;
+		}
+		case PLANE_BOTTOM: {
+			Plane new_plane = Plane(matrix[3] + matrix[1],
+					matrix[7] + matrix[5],
+					matrix[11] + matrix[9],
+					matrix[15] + matrix[13]);
+
+			new_plane.normal = -new_plane.normal;
+			new_plane.normalize();
+			return new_plane;
+		}
+	}
+
+	return Plane();
+}
+
+Projection Projection::flipped_y() const {
+	Projection proj = *this;
+	proj.flip_y();
+	return proj;
+}
+
+Projection Projection ::jitter_offseted(const Vector2 &p_offset) const {
+	Projection proj = *this;
+	proj.add_jitter_offset(p_offset);
+	return proj;
+}
+
+void Projection::set_perspective(real_t p_fovy_degrees, real_t p_aspect, real_t p_z_near, real_t p_z_far, bool p_flip_fov) {
+	if (p_flip_fov) {
+		p_fovy_degrees = get_fovy(p_fovy_degrees, 1.0 / p_aspect);
+	}
+
+	real_t sine, cotangent, deltaZ;
+	real_t radians = Math::deg_to_rad(p_fovy_degrees / 2.0);
+
+	deltaZ = p_z_far - p_z_near;
+	sine = Math::sin(radians);
+
+	if ((deltaZ == 0) || (sine == 0) || (p_aspect == 0)) {
+		return;
+	}
+	cotangent = Math::cos(radians) / sine;
+
+	set_identity();
+
+	columns[0][0] = cotangent / p_aspect;
+	columns[1][1] = cotangent;
+	columns[2][2] = -(p_z_far + p_z_near) / deltaZ;
+	columns[2][3] = -1;
+	columns[3][2] = -2 * p_z_near * p_z_far / deltaZ;
+	columns[3][3] = 0;
+}
+
+void Projection::set_perspective(real_t p_fovy_degrees, real_t p_aspect, real_t p_z_near, real_t p_z_far, bool p_flip_fov, int p_eye, real_t p_intraocular_dist, real_t p_convergence_dist) {
+	if (p_flip_fov) {
+		p_fovy_degrees = get_fovy(p_fovy_degrees, 1.0 / p_aspect);
+	}
+
+	real_t left, right, modeltranslation, ymax, xmax, frustumshift;
+
+	ymax = p_z_near * std::tan(Math::deg_to_rad(p_fovy_degrees / 2.0));
+	xmax = ymax * p_aspect;
+	frustumshift = (p_intraocular_dist / 2.0) * p_z_near / p_convergence_dist;
+
+	switch (p_eye) {
+		case 1: { // left eye
+			left = -xmax + frustumshift;
+			right = xmax + frustumshift;
+			modeltranslation = p_intraocular_dist / 2.0;
+		} break;
+		case 2: { // right eye
+			left = -xmax - frustumshift;
+			right = xmax - frustumshift;
+			modeltranslation = -p_intraocular_dist / 2.0;
+		} break;
+		default: { // mono, should give the same result as set_perspective(p_fovy_degrees,p_aspect,p_z_near,p_z_far,p_flip_fov)
+			left = -xmax;
+			right = xmax;
+			modeltranslation = 0.0;
+		} break;
+	}
+
+	set_frustum(left, right, -ymax, ymax, p_z_near, p_z_far);
+
+	// translate matrix by (modeltranslation, 0.0, 0.0)
+	Projection cm;
+	cm.set_identity();
+	cm.columns[3][0] = modeltranslation;
+	*this = *this * cm;
+}
+
+void Projection::set_for_hmd(int p_eye, real_t p_aspect, real_t p_intraocular_dist, real_t p_display_width, real_t p_display_to_lens, real_t p_oversample, real_t p_z_near, real_t p_z_far) {
+	// we first calculate our base frustum on our values without taking our lens magnification into account.
+	real_t f1 = (p_intraocular_dist * 0.5) / p_display_to_lens;
+	real_t f2 = ((p_display_width - p_intraocular_dist) * 0.5) / p_display_to_lens;
+	real_t f3 = (p_display_width / 4.0) / p_display_to_lens;
+
+	// now we apply our oversample factor to increase our FOV. how much we oversample is always a balance we strike between performance and how much
+	// we're willing to sacrifice in FOV.
+	real_t add = ((f1 + f2) * (p_oversample - 1.0)) / 2.0;
+	f1 += add;
+	f2 += add;
+	f3 *= p_oversample;
+
+	// always apply KEEP_WIDTH aspect ratio
+	f3 /= p_aspect;
+
+	switch (p_eye) {
+		case 1: { // left eye
+			set_frustum(-f2 * p_z_near, f1 * p_z_near, -f3 * p_z_near, f3 * p_z_near, p_z_near, p_z_far);
+		} break;
+		case 2: { // right eye
+			set_frustum(-f1 * p_z_near, f2 * p_z_near, -f3 * p_z_near, f3 * p_z_near, p_z_near, p_z_far);
+		} break;
+		default: { // mono, does not apply here!
+		} break;
+	}
+}
+
+void Projection::set_orthogonal(real_t p_left, real_t p_right, real_t p_bottom, real_t p_top, real_t p_znear, real_t p_zfar) {
+	set_identity();
+
+	columns[0][0] = 2.0 / (p_right - p_left);
+	columns[3][0] = -((p_right + p_left) / (p_right - p_left));
+	columns[1][1] = 2.0 / (p_top - p_bottom);
+	columns[3][1] = -((p_top + p_bottom) / (p_top - p_bottom));
+	columns[2][2] = -2.0 / (p_zfar - p_znear);
+	columns[3][2] = -((p_zfar + p_znear) / (p_zfar - p_znear));
+	columns[3][3] = 1.0;
+}
+
+void Projection::set_orthogonal(real_t p_size, real_t p_aspect, real_t p_znear, real_t p_zfar, bool p_flip_fov) {
+	if (!p_flip_fov) {
+		p_size *= p_aspect;
+	}
+
+	set_orthogonal(-p_size / 2, +p_size / 2, -p_size / p_aspect / 2, +p_size / p_aspect / 2, p_znear, p_zfar);
+}
+
+void Projection::set_frustum(real_t p_left, real_t p_right, real_t p_bottom, real_t p_top, real_t p_near, real_t p_far) {
+	ERR_FAIL_COND(p_right <= p_left);
+	ERR_FAIL_COND(p_top <= p_bottom);
+	ERR_FAIL_COND(p_far <= p_near);
+
+	real_t *te = &columns[0][0];
+	real_t x = 2 * p_near / (p_right - p_left);
+	real_t y = 2 * p_near / (p_top - p_bottom);
+
+	real_t a = (p_right + p_left) / (p_right - p_left);
+	real_t b = (p_top + p_bottom) / (p_top - p_bottom);
+	real_t c = -(p_far + p_near) / (p_far - p_near);
+	real_t d = -2 * p_far * p_near / (p_far - p_near);
+
+	te[0] = x;
+	te[1] = 0;
+	te[2] = 0;
+	te[3] = 0;
+	te[4] = 0;
+	te[5] = y;
+	te[6] = 0;
+	te[7] = 0;
+	te[8] = a;
+	te[9] = b;
+	te[10] = c;
+	te[11] = -1;
+	te[12] = 0;
+	te[13] = 0;
+	te[14] = d;
+	te[15] = 0;
+}
+
+void Projection::set_frustum(real_t p_size, real_t p_aspect, Vector2 p_offset, real_t p_near, real_t p_far, bool p_flip_fov) {
+	if (!p_flip_fov) {
+		p_size *= p_aspect;
+	}
+
+	set_frustum(-p_size / 2 + p_offset.x, +p_size / 2 + p_offset.x, -p_size / p_aspect / 2 + p_offset.y, +p_size / p_aspect / 2 + p_offset.y, p_near, p_far);
+}
+
+real_t Projection::get_z_far() const {
+	// NOTE: This assumes z-facing near and far planes, i.e. that :
+	// - the matrix is a projection across z-axis (i.e. is invertible and columns[0][1], [0][3], [1][0] and [1][3] == 0)
+	// - near and far planes are z-facing (i.e. columns[0][2] and [1][2] == 0)
+	return (columns[3][3] - columns[3][2]) / (columns[2][3] - columns[2][2]);
+}
+
+real_t Projection::get_z_near() const {
+	// NOTE: This assumes z-facing near and far planes, i.e. that :
+	// - the matrix is a projection across z-axis (i.e. is invertible and columns[0][1], [0][3], [1][0] and [1][3] == 0)
+	// - near and far planes are z-facing (i.e. columns[0][2] and [1][2] == 0)
+	return (columns[3][3] + columns[3][2]) / (columns[2][3] + columns[2][2]);
+}
+
+Vector2 Projection::get_viewport_half_extents() const {
+	// NOTE: This assumes a symmetrical frustum, i.e. that :
+	// - the matrix is a projection across z-axis (i.e. is invertible and columns[0][1], [0][3], [1][0] and [1][3] == 0)
+	// - the projection plane is rectangular (i.e. columns[0][2] and [1][2] == 0 if columns[2][3] != 0)
+	// - there is no offset / skew (i.e. columns[2][0] == columns[2][1] == 0)
+	real_t w = -get_z_near() * columns[2][3] + columns[3][3];
+	return Vector2(w / columns[0][0], w / columns[1][1]);
+}
+
+Vector2 Projection::get_far_plane_half_extents() const {
+	// NOTE: This assumes a symmetrical frustum, i.e. that :
+	// - the matrix is a projection across z-axis (i.e. is invertible and columns[0][1], [0][3], [1][0] and [1][3] == 0)
+	// - the projection plane is rectangular (i.e. columns[0][2] and [1][2] == 0 if columns[2][3] != 0)
+	// - there is no offset / skew (i.e. columns[2][0] == columns[2][1] == 0)
+	real_t w = -get_z_far() * columns[2][3] + columns[3][3];
+	return Vector2(w / columns[0][0], w / columns[1][1]);
+}
+
+bool Projection::get_endpoints(const Transform3D &p_transform, Vector3 *p_8points) const {
+	Vector<Plane> planes = get_projection_planes(Transform3D());
+	const Planes intersections[8][3] = {
+		{ PLANE_FAR, PLANE_LEFT, PLANE_TOP },
+		{ PLANE_FAR, PLANE_LEFT, PLANE_BOTTOM },
+		{ PLANE_FAR, PLANE_RIGHT, PLANE_TOP },
+		{ PLANE_FAR, PLANE_RIGHT, PLANE_BOTTOM },
+		{ PLANE_NEAR, PLANE_LEFT, PLANE_TOP },
+		{ PLANE_NEAR, PLANE_LEFT, PLANE_BOTTOM },
+		{ PLANE_NEAR, PLANE_RIGHT, PLANE_TOP },
+		{ PLANE_NEAR, PLANE_RIGHT, PLANE_BOTTOM },
+	};
+
+	for (int i = 0; i < 8; i++) {
+		Vector3 point;
+		Plane a = planes[intersections[i][0]];
+		Plane b = planes[intersections[i][1]];
+		Plane c = planes[intersections[i][2]];
+		bool res = a.intersect_3(b, c, &point);
+		ERR_FAIL_COND_V(!res, false);
+		p_8points[i] = p_transform.xform(point);
+	}
+
+	return true;
+}
+
+Vector<Plane> Projection::get_projection_planes(const Transform3D &p_transform) const {
+	/** Fast Plane Extraction from combined modelview/projection matrices.
+	 * References:
+	 * https://web.archive.org/web/20011221205252/https://www.markmorley.com/opengl/frustumculling.html
+	 * https://web.archive.org/web/20061020020112/https://www2.ravensoft.com/users/ggribb/plane%20extraction.pdf
+	 */
+
+	Vector<Plane> planes;
+	planes.resize(6);
+
+	const real_t *matrix = (const real_t *)columns;
+
+	Plane new_plane;
+
+	///////--- Near Plane ---///////
+	new_plane = Plane(matrix[3] + matrix[2],
+			matrix[7] + matrix[6],
+			matrix[11] + matrix[10],
+			matrix[15] + matrix[14]);
+
+	new_plane.normal = -new_plane.normal;
+	new_plane.normalize();
+
+	planes.write[0] = p_transform.xform(new_plane);
+
+	///////--- Far Plane ---///////
+	new_plane = Plane(matrix[3] - matrix[2],
+			matrix[7] - matrix[6],
+			matrix[11] - matrix[10],
+			matrix[15] - matrix[14]);
+
+	new_plane.normal = -new_plane.normal;
+	new_plane.normalize();
+
+	planes.write[1] = p_transform.xform(new_plane);
+
+	///////--- Left Plane ---///////
+	new_plane = Plane(matrix[3] + matrix[0],
+			matrix[7] + matrix[4],
+			matrix[11] + matrix[8],
+			matrix[15] + matrix[12]);
+
+	new_plane.normal = -new_plane.normal;
+	new_plane.normalize();
+
+	planes.write[2] = p_transform.xform(new_plane);
+
+	///////--- Top Plane ---///////
+	new_plane = Plane(matrix[3] - matrix[1],
+			matrix[7] - matrix[5],
+			matrix[11] - matrix[9],
+			matrix[15] - matrix[13]);
+
+	new_plane.normal = -new_plane.normal;
+	new_plane.normalize();
+
+	planes.write[3] = p_transform.xform(new_plane);
+
+	///////--- Right Plane ---///////
+	new_plane = Plane(matrix[3] - matrix[0],
+			matrix[7] - matrix[4],
+			matrix[11] - matrix[8],
+			matrix[15] - matrix[12]);
+
+	new_plane.normal = -new_plane.normal;
+	new_plane.normalize();
+
+	planes.write[4] = p_transform.xform(new_plane);
+
+	///////--- Bottom Plane ---///////
+	new_plane = Plane(matrix[3] + matrix[1],
+			matrix[7] + matrix[5],
+			matrix[11] + matrix[9],
+			matrix[15] + matrix[13]);
+
+	new_plane.normal = -new_plane.normal;
+	new_plane.normalize();
+
+	planes.write[5] = p_transform.xform(new_plane);
+
+	return planes;
+}
+
+Projection Projection::inverse() const {
+	Projection cm = *this;
+	cm.invert();
+	return cm;
+}
+
+void Projection::invert() {
+	// Adapted from Mesa's `src/util/u_math.c` `util_invert_mat4x4`.
+	// MIT licensed. Copyright 2008 VMware, Inc. Authored by Jacques Leroy.
+	Projection temp;
+	real_t *out = (real_t *)temp.columns;
+	real_t *m = (real_t *)columns;
+
+	real_t wtmp[4][8];
+	real_t m0, m1, m2, m3, s;
+	real_t *r0, *r1, *r2, *r3;
+
+#define MAT(m, r, c) (m)[(c) * 4 + (r)]
+
+	r0 = wtmp[0];
+	r1 = wtmp[1];
+	r2 = wtmp[2];
+	r3 = wtmp[3];
+
+	r0[0] = MAT(m, 0, 0);
+	r0[1] = MAT(m, 0, 1);
+	r0[2] = MAT(m, 0, 2);
+	r0[3] = MAT(m, 0, 3);
+	r0[4] = 1.0;
+	r0[5] = 0.0;
+	r0[6] = 0.0;
+	r0[7] = 0.0;
+
+	r1[0] = MAT(m, 1, 0);
+	r1[1] = MAT(m, 1, 1);
+	r1[2] = MAT(m, 1, 2);
+	r1[3] = MAT(m, 1, 3);
+	r1[5] = 1.0;
+	r1[4] = 0.0;
+	r1[6] = 0.0;
+	r1[7] = 0.0;
+
+	r2[0] = MAT(m, 2, 0);
+	r2[1] = MAT(m, 2, 1);
+	r2[2] = MAT(m, 2, 2);
+	r2[3] = MAT(m, 2, 3);
+	r2[6] = 1.0;
+	r2[4] = 0.0;
+	r2[5] = 0.0;
+	r2[7] = 0.0;
+
+	r3[0] = MAT(m, 3, 0);
+	r3[1] = MAT(m, 3, 1);
+	r3[2] = MAT(m, 3, 2);
+	r3[3] = MAT(m, 3, 3);
+
+	r3[7] = 1.0;
+	r3[4] = 0.0;
+	r3[5] = 0.0;
+	r3[6] = 0.0;
+
+	/* choose pivot - or die */
+	if (Math::abs(r3[0]) > Math::abs(r2[0])) {
+		SWAP(r3, r2);
+	}
+	if (Math::abs(r2[0]) > Math::abs(r1[0])) {
+		SWAP(r2, r1);
+	}
+	if (Math::abs(r1[0]) > Math::abs(r0[0])) {
+		SWAP(r1, r0);
+	}
+	ERR_FAIL_COND(0.0 == r0[0]);
+
+	/* eliminate first variable     */
+	m1 = r1[0] / r0[0];
+	m2 = r2[0] / r0[0];
+	m3 = r3[0] / r0[0];
+	s = r0[1];
+	r1[1] -= m1 * s;
+	r2[1] -= m2 * s;
+	r3[1] -= m3 * s;
+	s = r0[2];
+	r1[2] -= m1 * s;
+	r2[2] -= m2 * s;
+	r3[2] -= m3 * s;
+	s = r0[3];
+	r1[3] -= m1 * s;
+	r2[3] -= m2 * s;
+	r3[3] -= m3 * s;
+	s = r0[4];
+	if (s != 0.0) {
+		r1[4] -= m1 * s;
+		r2[4] -= m2 * s;
+		r3[4] -= m3 * s;
+	}
+	s = r0[5];
+	if (s != 0.0) {
+		r1[5] -= m1 * s;
+		r2[5] -= m2 * s;
+		r3[5] -= m3 * s;
+	}
+	s = r0[6];
+	if (s != 0.0) {
+		r1[6] -= m1 * s;
+		r2[6] -= m2 * s;
+		r3[6] -= m3 * s;
+	}
+	s = r0[7];
+	if (s != 0.0) {
+		r1[7] -= m1 * s;
+		r2[7] -= m2 * s;
+		r3[7] -= m3 * s;
+	}
+
+	/* choose pivot - or die */
+	if (Math::abs(r3[1]) > Math::abs(r2[1])) {
+		SWAP(r3, r2);
+	}
+	if (Math::abs(r2[1]) > Math::abs(r1[1])) {
+		SWAP(r2, r1);
+	}
+	ERR_FAIL_COND(0.0 == r1[1]);
+
+	/* eliminate second variable */
+	m2 = r2[1] / r1[1];
+	m3 = r3[1] / r1[1];
+	r2[2] -= m2 * r1[2];
+	r3[2] -= m3 * r1[2];
+	r2[3] -= m2 * r1[3];
+	r3[3] -= m3 * r1[3];
+	s = r1[4];
+	if (0.0 != s) {
+		r2[4] -= m2 * s;
+		r3[4] -= m3 * s;
+	}
+	s = r1[5];
+	if (0.0 != s) {
+		r2[5] -= m2 * s;
+		r3[5] -= m3 * s;
+	}
+	s = r1[6];
+	if (0.0 != s) {
+		r2[6] -= m2 * s;
+		r3[6] -= m3 * s;
+	}
+	s = r1[7];
+	if (0.0 != s) {
+		r2[7] -= m2 * s;
+		r3[7] -= m3 * s;
+	}
+
+	/* choose pivot - or die */
+	if (Math::abs(r3[2]) > Math::abs(r2[2])) {
+		SWAP(r3, r2);
+	}
+	ERR_FAIL_COND(0.0 == r2[2]);
+
+	/* eliminate third variable */
+	m3 = r3[2] / r2[2];
+	r3[3] -= m3 * r2[3];
+	r3[4] -= m3 * r2[4];
+	r3[5] -= m3 * r2[5];
+	r3[6] -= m3 * r2[6];
+	r3[7] -= m3 * r2[7];
+
+	/* last check */
+	ERR_FAIL_COND(0.0 == r3[3]);
+
+	s = 1.0 / r3[3]; /* now back substitute row 3 */
+	r3[4] *= s;
+	r3[5] *= s;
+	r3[6] *= s;
+	r3[7] *= s;
+
+	m2 = r2[3]; /* now back substitute row 2 */
+	s = 1.0 / r2[2];
+	r2[4] = s * (r2[4] - r3[4] * m2);
+	r2[5] = s * (r2[5] - r3[5] * m2);
+	r2[6] = s * (r2[6] - r3[6] * m2);
+	r2[7] = s * (r2[7] - r3[7] * m2);
+	m1 = r1[3];
+	r1[4] -= r3[4] * m1;
+	r1[5] -= r3[5] * m1;
+	r1[6] -= r3[6] * m1;
+	r1[7] -= r3[7] * m1;
+	m0 = r0[3];
+	r0[4] -= r3[4] * m0;
+	r0[5] -= r3[5] * m0;
+	r0[6] -= r3[6] * m0;
+	r0[7] -= r3[7] * m0;
+
+	m1 = r1[2]; /* now back substitute row 1 */
+	s = 1.0 / r1[1];
+	r1[4] = s * (r1[4] - r2[4] * m1);
+	r1[5] = s * (r1[5] - r2[5] * m1),
+	r1[6] = s * (r1[6] - r2[6] * m1);
+	r1[7] = s * (r1[7] - r2[7] * m1);
+	m0 = r0[2];
+	r0[4] -= r2[4] * m0;
+	r0[5] -= r2[5] * m0;
+	r0[6] -= r2[6] * m0;
+	r0[7] -= r2[7] * m0;
+
+	m0 = r0[1]; /* now back substitute row 0 */
+	s = 1.0 / r0[0];
+	r0[4] = s * (r0[4] - r1[4] * m0);
+	r0[5] = s * (r0[5] - r1[5] * m0),
+	r0[6] = s * (r0[6] - r1[6] * m0);
+	r0[7] = s * (r0[7] - r1[7] * m0);
+
+	MAT(out, 0, 0) = r0[4];
+	MAT(out, 0, 1) = r0[5];
+	MAT(out, 0, 2) = r0[6];
+	MAT(out, 0, 3) = r0[7];
+	MAT(out, 1, 0) = r1[4];
+	MAT(out, 1, 1) = r1[5];
+	MAT(out, 1, 2) = r1[6];
+	MAT(out, 1, 3) = r1[7];
+	MAT(out, 2, 0) = r2[4];
+	MAT(out, 2, 1) = r2[5];
+	MAT(out, 2, 2) = r2[6];
+	MAT(out, 2, 3) = r2[7];
+	MAT(out, 3, 0) = r3[4];
+	MAT(out, 3, 1) = r3[5];
+	MAT(out, 3, 2) = r3[6];
+	MAT(out, 3, 3) = r3[7];
+
+#undef MAT
+
+	*this = temp;
+}
+
+void Projection::flip_y() {
+	for (int i = 0; i < 4; i++) {
+		columns[1][i] = -columns[1][i];
+	}
+}
+
+bool Projection::is_same(const Projection &p_cam) const {
+	return columns[0].is_same(p_cam.columns[0]) && columns[1].is_same(p_cam.columns[1]) && columns[2].is_same(p_cam.columns[2]) && columns[3].is_same(p_cam.columns[3]);
+}
+
+void Projection::set_depth_correction(bool p_flip_y, bool p_reverse_z, bool p_remap_z) {
+	// p_remap_z is used to convert from OpenGL-style clip space (-1 - 1) to Vulkan style (0 - 1).
+	real_t *m = &columns[0][0];
+
+	m[0] = 1;
+	m[1] = 0.0;
+	m[2] = 0.0;
+	m[3] = 0.0;
+	m[4] = 0.0;
+	m[5] = p_flip_y ? -1 : 1;
+	m[6] = 0.0;
+	m[7] = 0.0;
+	m[8] = 0.0;
+	m[9] = 0.0;
+	m[10] = p_remap_z ? (p_reverse_z ? -0.5 : 0.5) : (p_reverse_z ? -1.0 : 1.0);
+	m[11] = 0.0;
+	m[12] = 0.0;
+	m[13] = 0.0;
+	m[14] = p_remap_z ? 0.5 : 0.0;
+	m[15] = 1.0;
+}
+
+void Projection::set_light_bias() {
+	real_t *m = &columns[0][0];
+
+	m[0] = 0.5;
+	m[1] = 0.0;
+	m[2] = 0.0;
+	m[3] = 0.0;
+	m[4] = 0.0;
+	m[5] = 0.5;
+	m[6] = 0.0;
+	m[7] = 0.0;
+	m[8] = 0.0;
+	m[9] = 0.0;
+	m[10] = 0.5;
+	m[11] = 0.0;
+	m[12] = 0.5;
+	m[13] = 0.5;
+	m[14] = 0.5;
+	m[15] = 1.0;
+}
+
+void Projection::set_light_atlas_rect(const Rect2 &p_rect) {
+	real_t *m = &columns[0][0];
+
+	m[0] = p_rect.size.width;
+	m[1] = 0.0;
+	m[2] = 0.0;
+	m[3] = 0.0;
+	m[4] = 0.0;
+	m[5] = p_rect.size.height;
+	m[6] = 0.0;
+	m[7] = 0.0;
+	m[8] = 0.0;
+	m[9] = 0.0;
+	m[10] = 1.0;
+	m[11] = 0.0;
+	m[12] = p_rect.position.x;
+	m[13] = p_rect.position.y;
+	m[14] = 0.0;
+	m[15] = 1.0;
+}
+
+Projection::operator String() const {
+	return "[X: " + columns[0].operator String() +
+			", Y: " + columns[1].operator String() +
+			", Z: " + columns[2].operator String() +
+			", W: " + columns[3].operator String() + "]";
+}
+
+real_t Projection::get_aspect() const {
+	// NOTE: This assumes a rectangular projection plane, i.e. that :
+	// - the matrix is a projection across z-axis (i.e. is invertible and columns[0][1], [0][3], [1][0] and [1][3] == 0)
+	// - the projection plane is rectangular (i.e. columns[0][2] and [1][2] == 0 if columns[2][3] != 0)
+	return columns[1][1] / columns[0][0];
+}
+
+int Projection::get_pixels_per_meter(int p_for_pixel_width) const {
+	// NOTE: This assumes a rectangular projection plane, i.e. that :
+	// - the matrix is a projection across z-axis (i.e. is invertible and columns[0][1], [0][3], [1][0] and [1][3] == 0)
+	// - the projection plane is rectangular (i.e. columns[0][2] and [1][2] == 0 if columns[2][3] != 0)
+	real_t width = 2 * (-get_z_near() * columns[2][3] + columns[3][3]) / columns[0][0];
+	return p_for_pixel_width / width; // Note : return type should be real_t (kept as int for compatibility for now).
+}
+
+bool Projection::is_orthogonal() const {
+	// NOTE: This assumes that the matrix is a projection across z-axis
+	// i.e. is invertible and columns[0][1], [0][3], [1][0] and [1][3] == 0
+	return columns[2][3] == 0.0;
+}
+
+real_t Projection::get_fov() const {
+	// NOTE: This assumes a rectangular projection plane, i.e. that :
+	// - the matrix is a projection across z-axis (i.e. is invertible and columns[0][1], [0][3], [1][0] and [1][3] == 0)
+	// - the projection plane is rectangular (i.e. columns[0][2] and [1][2] == 0 if columns[2][3] != 0)
+	if (columns[2][0] == 0) {
+		return Math::rad_to_deg(2 * Math::atan2(1, columns[0][0]));
+	} else {
+		// The frustum is asymmetrical so we need to calculate the left and right angles separately.
+		real_t right = Math::atan2(columns[2][0] + 1, columns[0][0]);
+		real_t left = Math::atan2(columns[2][0] - 1, columns[0][0]);
+		return Math::rad_to_deg(right - left);
+	}
+}
+
+real_t Projection::get_lod_multiplier() const {
+	// NOTE: This assumes a rectangular projection plane, i.e. that :
+	// - the matrix is a projection across z-axis (i.e. is invertible and columns[0][1], [0][3], [1][0] and [1][3] == 0)
+	// - the projection plane is rectangular (i.e. columns[0][2] and [1][2] == 0 if columns[2][3] != 0)
+	return 2 / columns[0][0];
+}
+
+void Projection::make_scale(const Vector3 &p_scale) {
+	set_identity();
+	columns[0][0] = p_scale.x;
+	columns[1][1] = p_scale.y;
+	columns[2][2] = p_scale.z;
+}
+
+void Projection::scale_translate_to_fit(const AABB &p_aabb) {
+	Vector3 min = p_aabb.position;
+	Vector3 max = p_aabb.position + p_aabb.size;
+
+	columns[0][0] = 2 / (max.x - min.x);
+	columns[1][0] = 0;
+	columns[2][0] = 0;
+	columns[3][0] = -(max.x + min.x) / (max.x - min.x);
+
+	columns[0][1] = 0;
+	columns[1][1] = 2 / (max.y - min.y);
+	columns[2][1] = 0;
+	columns[3][1] = -(max.y + min.y) / (max.y - min.y);
+
+	columns[0][2] = 0;
+	columns[1][2] = 0;
+	columns[2][2] = 2 / (max.z - min.z);
+	columns[3][2] = -(max.z + min.z) / (max.z - min.z);
+
+	columns[0][3] = 0;
+	columns[1][3] = 0;
+	columns[2][3] = 0;
+	columns[3][3] = 1;
+}
+
+void Projection::add_jitter_offset(const Vector2 &p_offset) {
+	columns[3][0] += p_offset.x;
+	columns[3][1] += p_offset.y;
+}
+
+Projection::operator Transform3D() const {
+	Transform3D tr;
+	const real_t *m = &columns[0][0];
+
+	tr.basis.rows[0][0] = m[0];
+	tr.basis.rows[1][0] = m[1];
+	tr.basis.rows[2][0] = m[2];
+
+	tr.basis.rows[0][1] = m[4];
+	tr.basis.rows[1][1] = m[5];
+	tr.basis.rows[2][1] = m[6];
+
+	tr.basis.rows[0][2] = m[8];
+	tr.basis.rows[1][2] = m[9];
+	tr.basis.rows[2][2] = m[10];
+
+	tr.origin.x = m[12];
+	tr.origin.y = m[13];
+	tr.origin.z = m[14];
+
+	return tr;
+}
+
+Projection::Projection(const Transform3D &p_transform) {
+	const Transform3D &tr = p_transform;
+	real_t *m = &columns[0][0];
+
+	m[0] = tr.basis.rows[0][0];
+	m[1] = tr.basis.rows[1][0];
+	m[2] = tr.basis.rows[2][0];
+	m[3] = 0.0;
+	m[4] = tr.basis.rows[0][1];
+	m[5] = tr.basis.rows[1][1];
+	m[6] = tr.basis.rows[2][1];
+	m[7] = 0.0;
+	m[8] = tr.basis.rows[0][2];
+	m[9] = tr.basis.rows[1][2];
+	m[10] = tr.basis.rows[2][2];
+	m[11] = 0.0;
+	m[12] = tr.origin.x;
+	m[13] = tr.origin.y;
+	m[14] = tr.origin.z;
+	m[15] = 1.0;
+}

core/math/projection.h

@@ -0,0 +1,195 @@
+/**************************************************************************/
+/*  projection.h                                                          */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "core/math/vector3.h"
+#include "core/math/vector4.h"
+
+template <typename T>
+class Vector;
+
+struct AABB;
+struct Plane;
+struct Rect2;
+struct Transform3D;
+struct Vector2;
+
+struct [[nodiscard]] Projection {
+	enum Planes {
+		PLANE_NEAR,
+		PLANE_FAR,
+		PLANE_LEFT,
+		PLANE_TOP,
+		PLANE_RIGHT,
+		PLANE_BOTTOM
+	};
+
+	Vector4 columns[4] = {
+		{ 1, 0, 0, 0 },
+		{ 0, 1, 0, 0 },
+		{ 0, 0, 1, 0 },
+		{ 0, 0, 0, 1 },
+	};
+
+	constexpr const Vector4 &operator[](int p_axis) const {
+		DEV_ASSERT((unsigned int)p_axis < 4);
+		return columns[p_axis];
+	}
+
+	constexpr Vector4 &operator[](int p_axis) {
+		DEV_ASSERT((unsigned int)p_axis < 4);
+		return columns[p_axis];
+	}
+
+	real_t determinant() const;
+	void set_identity();
+	void set_zero();
+	void set_light_bias();
+	void set_depth_correction(bool p_flip_y = true, bool p_reverse_z = true, bool p_remap_z = true);
+
+	void set_light_atlas_rect(const Rect2 &p_rect);
+	void set_perspective(real_t p_fovy_degrees, real_t p_aspect, real_t p_z_near, real_t p_z_far, bool p_flip_fov = false);
+	void set_perspective(real_t p_fovy_degrees, real_t p_aspect, real_t p_z_near, real_t p_z_far, bool p_flip_fov, int p_eye, real_t p_intraocular_dist, real_t p_convergence_dist);
+	void set_for_hmd(int p_eye, real_t p_aspect, real_t p_intraocular_dist, real_t p_display_width, real_t p_display_to_lens, real_t p_oversample, real_t p_z_near, real_t p_z_far);
+	void set_orthogonal(real_t p_left, real_t p_right, real_t p_bottom, real_t p_top, real_t p_znear, real_t p_zfar);
+	void set_orthogonal(real_t p_size, real_t p_aspect, real_t p_znear, real_t p_zfar, bool p_flip_fov = false);
+	void set_frustum(real_t p_left, real_t p_right, real_t p_bottom, real_t p_top, real_t p_near, real_t p_far);
+	void set_frustum(real_t p_size, real_t p_aspect, Vector2 p_offset, real_t p_near, real_t p_far, bool p_flip_fov = false);
+	void adjust_perspective_znear(real_t p_new_znear);
+
+	static Projection create_depth_correction(bool p_flip_y);
+	static Projection create_light_atlas_rect(const Rect2 &p_rect);
+	static Projection create_perspective(real_t p_fovy_degrees, real_t p_aspect, real_t p_z_near, real_t p_z_far, bool p_flip_fov = false);
+	static Projection create_perspective_hmd(real_t p_fovy_degrees, real_t p_aspect, real_t p_z_near, real_t p_z_far, bool p_flip_fov, int p_eye, real_t p_intraocular_dist, real_t p_convergence_dist);
+	static Projection create_for_hmd(int p_eye, real_t p_aspect, real_t p_intraocular_dist, real_t p_display_width, real_t p_display_to_lens, real_t p_oversample, real_t p_z_near, real_t p_z_far);
+	static Projection create_orthogonal(real_t p_left, real_t p_right, real_t p_bottom, real_t p_top, real_t p_znear, real_t p_zfar);
+	static Projection create_orthogonal_aspect(real_t p_size, real_t p_aspect, real_t p_znear, real_t p_zfar, bool p_flip_fov = false);
+	static Projection create_frustum(real_t p_left, real_t p_right, real_t p_bottom, real_t p_top, real_t p_near, real_t p_far);
+	static Projection create_frustum_aspect(real_t p_size, real_t p_aspect, Vector2 p_offset, real_t p_near, real_t p_far, bool p_flip_fov = false);
+	static Projection create_fit_aabb(const AABB &p_aabb);
+	Projection perspective_znear_adjusted(real_t p_new_znear) const;
+	Plane get_projection_plane(Planes p_plane) const;
+	Projection flipped_y() const;
+	Projection jitter_offseted(const Vector2 &p_offset) const;
+
+	static real_t get_fovy(real_t p_fovx, real_t p_aspect) {
+		return Math::rad_to_deg(Math::atan(p_aspect * Math::tan(Math::deg_to_rad(p_fovx) * 0.5)) * 2.0);
+	}
+
+	real_t get_z_far() const;
+	real_t get_z_near() const;
+	real_t get_aspect() const;
+	real_t get_fov() const;
+	bool is_orthogonal() const;
+
+	Vector<Plane> get_projection_planes(const Transform3D &p_transform) const;
+
+	bool get_endpoints(const Transform3D &p_transform, Vector3 *p_8points) const;
+	Vector2 get_viewport_half_extents() const;
+	Vector2 get_far_plane_half_extents() const;
+
+	void invert();
+	Projection inverse() const;
+
+	constexpr Projection operator*(const Projection &p_matrix) const;
+
+	Plane xform4(const Plane &p_vec4) const;
+	_FORCE_INLINE_ Vector3 xform(const Vector3 &p_vec3) const;
+
+	Vector4 xform(const Vector4 &p_vec4) const;
+	Vector4 xform_inv(const Vector4 &p_vec4) const;
+
+	explicit operator String() const;
+
+	void scale_translate_to_fit(const AABB &p_aabb);
+	void add_jitter_offset(const Vector2 &p_offset);
+	void make_scale(const Vector3 &p_scale);
+	int get_pixels_per_meter(int p_for_pixel_width) const;
+	operator Transform3D() const;
+
+	void flip_y();
+
+	bool is_same(const Projection &p_cam) const;
+
+	constexpr bool operator==(const Projection &p_cam) const {
+		for (uint32_t i = 0; i < 4; i++) {
+			for (uint32_t j = 0; j < 4; j++) {
+				if (columns[i][j] != p_cam.columns[i][j]) {
+					return false;
+				}
+			}
+		}
+		return true;
+	}
+
+	constexpr bool operator!=(const Projection &p_cam) const {
+		return !(*this == p_cam);
+	}
+
+	real_t get_lod_multiplier() const;
+
+	Projection() = default;
+	constexpr Projection(const Vector4 &p_x, const Vector4 &p_y, const Vector4 &p_z, const Vector4 &p_w) :
+			columns{ p_x, p_y, p_z, p_w } {}
+	constexpr Projection(real_t p_xx, real_t p_xy, real_t p_xz, real_t p_xw, real_t p_yx, real_t p_yy, real_t p_yz, real_t p_yw, real_t p_zx, real_t p_zy, real_t p_zz, real_t p_zw, real_t p_wx, real_t p_wy, real_t p_wz, real_t p_ww) :
+			columns{
+				{ p_xx, p_xy, p_xz, p_xw },
+				{ p_yx, p_yy, p_yz, p_yw },
+				{ p_zx, p_zy, p_zz, p_zw },
+				{ p_wx, p_wy, p_wz, p_ww },
+			} {}
+	Projection(const Transform3D &p_transform);
+};
+
+constexpr Projection Projection::operator*(const Projection &p_matrix) const {
+	Projection new_matrix;
+
+	for (int j = 0; j < 4; j++) {
+		for (int i = 0; i < 4; i++) {
+			real_t ab = 0;
+			for (int k = 0; k < 4; k++) {
+				ab += columns[k][i] * p_matrix.columns[j][k];
+			}
+			new_matrix.columns[j][i] = ab;
+		}
+	}
+
+	return new_matrix;
+}
+
+Vector3 Projection::xform(const Vector3 &p_vec3) const {
+	Vector3 ret;
+	ret.x = columns[0][0] * p_vec3.x + columns[1][0] * p_vec3.y + columns[2][0] * p_vec3.z + columns[3][0];
+	ret.y = columns[0][1] * p_vec3.x + columns[1][1] * p_vec3.y + columns[2][1] * p_vec3.z + columns[3][1];
+	ret.z = columns[0][2] * p_vec3.x + columns[1][2] * p_vec3.y + columns[2][2] * p_vec3.z + columns[3][2];
+	real_t w = columns[0][3] * p_vec3.x + columns[1][3] * p_vec3.y + columns[2][3] * p_vec3.z + columns[3][3];
+	return ret / w;
+}

core/math/quaternion.cpp

@@ -0,0 +1,329 @@
+/**************************************************************************/
+/*  quaternion.cpp                                                        */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "quaternion.h"
+
+#include "core/math/basis.h"
+#include "core/string/ustring.h"
+
+real_t Quaternion::angle_to(const Quaternion &p_to) const {
+	real_t d = dot(p_to);
+	// acos does clamping.
+	return Math::acos(d * d * 2 - 1);
+}
+
+Vector3 Quaternion::get_euler(EulerOrder p_order) const {
+#ifdef MATH_CHECKS
+	ERR_FAIL_COND_V_MSG(!is_normalized(), Vector3(0, 0, 0), "The quaternion " + operator String() + " must be normalized.");
+#endif
+	return Basis(*this).get_euler(p_order);
+}
+
+bool Quaternion::is_equal_approx(const Quaternion &p_quaternion) const {
+	return Math::is_equal_approx(x, p_quaternion.x) && Math::is_equal_approx(y, p_quaternion.y) && Math::is_equal_approx(z, p_quaternion.z) && Math::is_equal_approx(w, p_quaternion.w);
+}
+
+bool Quaternion::is_same(const Quaternion &p_quaternion) const {
+	return Math::is_same(x, p_quaternion.x) && Math::is_same(y, p_quaternion.y) && Math::is_same(z, p_quaternion.z) && Math::is_same(w, p_quaternion.w);
+}
+
+bool Quaternion::is_finite() const {
+	return Math::is_finite(x) && Math::is_finite(y) && Math::is_finite(z) && Math::is_finite(w);
+}
+
+real_t Quaternion::length() const {
+	return Math::sqrt(length_squared());
+}
+
+void Quaternion::normalize() {
+	*this /= length();
+}
+
+Quaternion Quaternion::normalized() const {
+	return *this / length();
+}
+
+bool Quaternion::is_normalized() const {
+	return Math::is_equal_approx(length_squared(), 1, (real_t)UNIT_EPSILON); //use less epsilon
+}
+
+Quaternion Quaternion::inverse() const {
+#ifdef MATH_CHECKS
+	ERR_FAIL_COND_V_MSG(!is_normalized(), Quaternion(), "The quaternion " + operator String() + " must be normalized.");
+#endif
+	return Quaternion(-x, -y, -z, w);
+}
+
+Quaternion Quaternion::log() const {
+	Quaternion src = *this;
+	Vector3 src_v = src.get_axis() * src.get_angle();
+	return Quaternion(src_v.x, src_v.y, src_v.z, 0);
+}
+
+Quaternion Quaternion::exp() const {
+	Quaternion src = *this;
+	Vector3 src_v = Vector3(src.x, src.y, src.z);
+	real_t theta = src_v.length();
+	src_v = src_v.normalized();
+	if (theta < CMP_EPSILON || !src_v.is_normalized()) {
+		return Quaternion(0, 0, 0, 1);
+	}
+	return Quaternion(src_v, theta);
+}
+
+Quaternion Quaternion::slerp(const Quaternion &p_to, real_t p_weight) const {
+#ifdef MATH_CHECKS
+	ERR_FAIL_COND_V_MSG(!is_normalized(), Quaternion(), "The start quaternion " + operator String() + " must be normalized.");
+	ERR_FAIL_COND_V_MSG(!p_to.is_normalized(), Quaternion(), "The end quaternion " + p_to.operator String() + " must be normalized.");
+#endif
+	Quaternion to1;
+	real_t omega, cosom, sinom, scale0, scale1;
+
+	// calc cosine
+	cosom = dot(p_to);
+
+	// adjust signs (if necessary)
+	if (cosom < 0.0f) {
+		cosom = -cosom;
+		to1 = -p_to;
+	} else {
+		to1 = p_to;
+	}
+
+	// calculate coefficients
+
+	if ((1.0f - cosom) > (real_t)CMP_EPSILON) {
+		// standard case (slerp)
+		omega = Math::acos(cosom);
+		sinom = Math::sin(omega);
+		scale0 = Math::sin((1.0 - p_weight) * omega) / sinom;
+		scale1 = Math::sin(p_weight * omega) / sinom;
+	} else {
+		// "from" and "to" quaternions are very close
+		//  ... so we can do a linear interpolation
+		scale0 = 1.0f - p_weight;
+		scale1 = p_weight;
+	}
+	// calculate final values
+	return Quaternion(
+			scale0 * x + scale1 * to1.x,
+			scale0 * y + scale1 * to1.y,
+			scale0 * z + scale1 * to1.z,
+			scale0 * w + scale1 * to1.w);
+}
+
+Quaternion Quaternion::slerpni(const Quaternion &p_to, real_t p_weight) const {
+#ifdef MATH_CHECKS
+	ERR_FAIL_COND_V_MSG(!is_normalized(), Quaternion(), "The start quaternion " + operator String() + " must be normalized.");
+	ERR_FAIL_COND_V_MSG(!p_to.is_normalized(), Quaternion(), "The end quaternion " + p_to.operator String() + " must be normalized.");
+#endif
+	const Quaternion &from = *this;
+
+	real_t dot = from.dot(p_to);
+
+	if (Math::abs(dot) > 0.9999f) {
+		return from;
+	}
+
+	real_t theta = Math::acos(dot),
+		   sinT = 1.0f / Math::sin(theta),
+		   newFactor = Math::sin(p_weight * theta) * sinT,
+		   invFactor = Math::sin((1.0f - p_weight) * theta) * sinT;
+
+	return Quaternion(invFactor * from.x + newFactor * p_to.x,
+			invFactor * from.y + newFactor * p_to.y,
+			invFactor * from.z + newFactor * p_to.z,
+			invFactor * from.w + newFactor * p_to.w);
+}
+
+Quaternion Quaternion::spherical_cubic_interpolate(const Quaternion &p_b, const Quaternion &p_pre_a, const Quaternion &p_post_b, real_t p_weight) const {
+#ifdef MATH_CHECKS
+	ERR_FAIL_COND_V_MSG(!is_normalized(), Quaternion(), "The start quaternion " + operator String() + " must be normalized.");
+	ERR_FAIL_COND_V_MSG(!p_b.is_normalized(), Quaternion(), "The end quaternion " + p_b.operator String() + " must be normalized.");
+#endif
+	Quaternion from_q = *this;
+	Quaternion pre_q = p_pre_a;
+	Quaternion to_q = p_b;
+	Quaternion post_q = p_post_b;
+
+	// Align flip phases.
+	from_q = Basis(from_q).get_rotation_quaternion();
+	pre_q = Basis(pre_q).get_rotation_quaternion();
+	to_q = Basis(to_q).get_rotation_quaternion();
+	post_q = Basis(post_q).get_rotation_quaternion();
+
+	// Flip quaternions to shortest path if necessary.
+	bool flip1 = std::signbit(from_q.dot(pre_q));
+	pre_q = flip1 ? -pre_q : pre_q;
+	bool flip2 = std::signbit(from_q.dot(to_q));
+	to_q = flip2 ? -to_q : to_q;
+	bool flip3 = flip2 ? to_q.dot(post_q) <= 0 : std::signbit(to_q.dot(post_q));
+	post_q = flip3 ? -post_q : post_q;
+
+	// Calc by Expmap in from_q space.
+	Quaternion ln_from = Quaternion(0, 0, 0, 0);
+	Quaternion ln_to = (from_q.inverse() * to_q).log();
+	Quaternion ln_pre = (from_q.inverse() * pre_q).log();
+	Quaternion ln_post = (from_q.inverse() * post_q).log();
+	Quaternion ln = Quaternion(0, 0, 0, 0);
+	ln.x = Math::cubic_interpolate(ln_from.x, ln_to.x, ln_pre.x, ln_post.x, p_weight);
+	ln.y = Math::cubic_interpolate(ln_from.y, ln_to.y, ln_pre.y, ln_post.y, p_weight);
+	ln.z = Math::cubic_interpolate(ln_from.z, ln_to.z, ln_pre.z, ln_post.z, p_weight);
+	Quaternion q1 = from_q * ln.exp();
+
+	// Calc by Expmap in to_q space.
+	ln_from = (to_q.inverse() * from_q).log();
+	ln_to = Quaternion(0, 0, 0, 0);
+	ln_pre = (to_q.inverse() * pre_q).log();
+	ln_post = (to_q.inverse() * post_q).log();
+	ln = Quaternion(0, 0, 0, 0);
+	ln.x = Math::cubic_interpolate(ln_from.x, ln_to.x, ln_pre.x, ln_post.x, p_weight);
+	ln.y = Math::cubic_interpolate(ln_from.y, ln_to.y, ln_pre.y, ln_post.y, p_weight);
+	ln.z = Math::cubic_interpolate(ln_from.z, ln_to.z, ln_pre.z, ln_post.z, p_weight);
+	Quaternion q2 = to_q * ln.exp();
+
+	// To cancel error made by Expmap ambiguity, do blending.
+	return q1.slerp(q2, p_weight);
+}
+
+Quaternion Quaternion::spherical_cubic_interpolate_in_time(const Quaternion &p_b, const Quaternion &p_pre_a, const Quaternion &p_post_b, real_t p_weight,
+		real_t p_b_t, real_t p_pre_a_t, real_t p_post_b_t) const {
+#ifdef MATH_CHECKS
+	ERR_FAIL_COND_V_MSG(!is_normalized(), Quaternion(), "The start quaternion " + operator String() + " must be normalized.");
+	ERR_FAIL_COND_V_MSG(!p_b.is_normalized(), Quaternion(), "The end quaternion " + p_b.operator String() + " must be normalized.");
+#endif
+	Quaternion from_q = *this;
+	Quaternion pre_q = p_pre_a;
+	Quaternion to_q = p_b;
+	Quaternion post_q = p_post_b;
+
+	// Align flip phases.
+	from_q = Basis(from_q).get_rotation_quaternion();
+	pre_q = Basis(pre_q).get_rotation_quaternion();
+	to_q = Basis(to_q).get_rotation_quaternion();
+	post_q = Basis(post_q).get_rotation_quaternion();
+
+	// Flip quaternions to shortest path if necessary.
+	bool flip1 = std::signbit(from_q.dot(pre_q));
+	pre_q = flip1 ? -pre_q : pre_q;
+	bool flip2 = std::signbit(from_q.dot(to_q));
+	to_q = flip2 ? -to_q : to_q;
+	bool flip3 = flip2 ? to_q.dot(post_q) <= 0 : std::signbit(to_q.dot(post_q));
+	post_q = flip3 ? -post_q : post_q;
+
+	// Calc by Expmap in from_q space.
+	Quaternion ln_from = Quaternion(0, 0, 0, 0);
+	Quaternion ln_to = (from_q.inverse() * to_q).log();
+	Quaternion ln_pre = (from_q.inverse() * pre_q).log();
+	Quaternion ln_post = (from_q.inverse() * post_q).log();
+	Quaternion ln = Quaternion(0, 0, 0, 0);
+	ln.x = Math::cubic_interpolate_in_time(ln_from.x, ln_to.x, ln_pre.x, ln_post.x, p_weight, p_b_t, p_pre_a_t, p_post_b_t);
+	ln.y = Math::cubic_interpolate_in_time(ln_from.y, ln_to.y, ln_pre.y, ln_post.y, p_weight, p_b_t, p_pre_a_t, p_post_b_t);
+	ln.z = Math::cubic_interpolate_in_time(ln_from.z, ln_to.z, ln_pre.z, ln_post.z, p_weight, p_b_t, p_pre_a_t, p_post_b_t);
+	Quaternion q1 = from_q * ln.exp();
+
+	// Calc by Expmap in to_q space.
+	ln_from = (to_q.inverse() * from_q).log();
+	ln_to = Quaternion(0, 0, 0, 0);
+	ln_pre = (to_q.inverse() * pre_q).log();
+	ln_post = (to_q.inverse() * post_q).log();
+	ln = Quaternion(0, 0, 0, 0);
+	ln.x = Math::cubic_interpolate_in_time(ln_from.x, ln_to.x, ln_pre.x, ln_post.x, p_weight, p_b_t, p_pre_a_t, p_post_b_t);
+	ln.y = Math::cubic_interpolate_in_time(ln_from.y, ln_to.y, ln_pre.y, ln_post.y, p_weight, p_b_t, p_pre_a_t, p_post_b_t);
+	ln.z = Math::cubic_interpolate_in_time(ln_from.z, ln_to.z, ln_pre.z, ln_post.z, p_weight, p_b_t, p_pre_a_t, p_post_b_t);
+	Quaternion q2 = to_q * ln.exp();
+
+	// To cancel error made by Expmap ambiguity, do blending.
+	return q1.slerp(q2, p_weight);
+}
+
+Quaternion::operator String() const {
+	return "(" + String::num_real(x, false) + ", " + String::num_real(y, false) + ", " + String::num_real(z, false) + ", " + String::num_real(w, false) + ")";
+}
+
+Vector3 Quaternion::get_axis() const {
+	if (Math::abs(w) > 1 - CMP_EPSILON) {
+		return Vector3(x, y, z);
+	}
+	real_t r = ((real_t)1) / Math::sqrt(1 - w * w);
+	return Vector3(x * r, y * r, z * r);
+}
+
+real_t Quaternion::get_angle() const {
+	return 2 * Math::acos(w);
+}
+
+Quaternion::Quaternion(const Vector3 &p_axis, real_t p_angle) {
+#ifdef MATH_CHECKS
+	ERR_FAIL_COND_MSG(!p_axis.is_normalized(), "The axis Vector3 " + p_axis.operator String() + " must be normalized.");
+#endif
+	real_t d = p_axis.length();
+	if (d == 0) {
+		x = 0;
+		y = 0;
+		z = 0;
+		w = 0;
+	} else {
+		real_t sin_angle = Math::sin(p_angle * 0.5f);
+		real_t cos_angle = Math::cos(p_angle * 0.5f);
+		real_t s = sin_angle / d;
+		x = p_axis.x * s;
+		y = p_axis.y * s;
+		z = p_axis.z * s;
+		w = cos_angle;
+	}
+}
+
+// Euler constructor expects a vector containing the Euler angles in the format
+// (ax, ay, az), where ax is the angle of rotation around x axis,
+// and similar for other axes.
+// This implementation uses YXZ convention (Z is the first rotation).
+Quaternion Quaternion::from_euler(const Vector3 &p_euler) {
+	real_t half_a1 = p_euler.y * 0.5f;
+	real_t half_a2 = p_euler.x * 0.5f;
+	real_t half_a3 = p_euler.z * 0.5f;
+
+	// R = Y(a1).X(a2).Z(a3) convention for Euler angles.
+	// Conversion to quaternion as listed in https://ntrs.nasa.gov/archive/nasa/casi.ntrs.nasa.gov/19770024290.pdf (page A-6)
+	// a3 is the angle of the first rotation, following the notation in this reference.
+
+	real_t cos_a1 = Math::cos(half_a1);
+	real_t sin_a1 = Math::sin(half_a1);
+	real_t cos_a2 = Math::cos(half_a2);
+	real_t sin_a2 = Math::sin(half_a2);
+	real_t cos_a3 = Math::cos(half_a3);
+	real_t sin_a3 = Math::sin(half_a3);
+
+	return Quaternion(
+			sin_a1 * cos_a2 * sin_a3 + cos_a1 * sin_a2 * cos_a3,
+			sin_a1 * cos_a2 * cos_a3 - cos_a1 * sin_a2 * sin_a3,
+			-sin_a1 * sin_a2 * cos_a3 + cos_a1 * cos_a2 * sin_a3,
+			sin_a1 * sin_a2 * sin_a3 + cos_a1 * cos_a2 * cos_a3);
+}

core/math/quaternion.h

@@ -0,0 +1,255 @@
+/**************************************************************************/
+/*  quaternion.h                                                          */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "core/math/math_funcs.h"
+#include "core/math/vector3.h"
+#include "core/string/ustring.h"
+
+struct [[nodiscard]] Quaternion {
+	union {
+		// NOLINTBEGIN(modernize-use-default-member-init)
+		struct {
+			real_t x;
+			real_t y;
+			real_t z;
+			real_t w;
+		};
+		real_t components[4] = { 0, 0, 0, 1.0 };
+		// NOLINTEND(modernize-use-default-member-init)
+	};
+
+	_FORCE_INLINE_ real_t &operator[](int p_idx) {
+		return components[p_idx];
+	}
+	_FORCE_INLINE_ const real_t &operator[](int p_idx) const {
+		return components[p_idx];
+	}
+	_FORCE_INLINE_ real_t length_squared() const;
+	bool is_equal_approx(const Quaternion &p_quaternion) const;
+	bool is_same(const Quaternion &p_quaternion) const;
+	bool is_finite() const;
+	real_t length() const;
+	void normalize();
+	Quaternion normalized() const;
+	bool is_normalized() const;
+	Quaternion inverse() const;
+	Quaternion log() const;
+	Quaternion exp() const;
+	_FORCE_INLINE_ real_t dot(const Quaternion &p_q) const;
+	real_t angle_to(const Quaternion &p_to) const;
+
+	Vector3 get_euler(EulerOrder p_order = EulerOrder::YXZ) const;
+	static Quaternion from_euler(const Vector3 &p_euler);
+
+	Quaternion slerp(const Quaternion &p_to, real_t p_weight) const;
+	Quaternion slerpni(const Quaternion &p_to, real_t p_weight) const;
+	Quaternion spherical_cubic_interpolate(const Quaternion &p_b, const Quaternion &p_pre_a, const Quaternion &p_post_b, real_t p_weight) const;
+	Quaternion spherical_cubic_interpolate_in_time(const Quaternion &p_b, const Quaternion &p_pre_a, const Quaternion &p_post_b, real_t p_weight, real_t p_b_t, real_t p_pre_a_t, real_t p_post_b_t) const;
+
+	Vector3 get_axis() const;
+	real_t get_angle() const;
+
+	_FORCE_INLINE_ void get_axis_angle(Vector3 &r_axis, real_t &r_angle) const {
+		r_angle = 2 * Math::acos(w);
+		real_t r = ((real_t)1) / Math::sqrt(1 - w * w);
+		r_axis.x = x * r;
+		r_axis.y = y * r;
+		r_axis.z = z * r;
+	}
+
+	constexpr void operator*=(const Quaternion &p_q);
+	constexpr Quaternion operator*(const Quaternion &p_q) const;
+
+	_FORCE_INLINE_ Vector3 xform(const Vector3 &p_v) const {
+#ifdef MATH_CHECKS
+		ERR_FAIL_COND_V_MSG(!is_normalized(), p_v, "The quaternion " + operator String() + " must be normalized.");
+#endif
+		Vector3 u(x, y, z);
+		Vector3 uv = u.cross(p_v);
+		return p_v + ((uv * w) + u.cross(uv)) * ((real_t)2);
+	}
+
+	_FORCE_INLINE_ Vector3 xform_inv(const Vector3 &p_v) const {
+		return inverse().xform(p_v);
+	}
+
+	constexpr void operator+=(const Quaternion &p_q);
+	constexpr void operator-=(const Quaternion &p_q);
+	constexpr void operator*=(real_t p_s);
+	constexpr void operator/=(real_t p_s);
+	constexpr Quaternion operator+(const Quaternion &p_q2) const;
+	constexpr Quaternion operator-(const Quaternion &p_q2) const;
+	constexpr Quaternion operator-() const;
+	constexpr Quaternion operator*(real_t p_s) const;
+	constexpr Quaternion operator/(real_t p_s) const;
+
+	constexpr bool operator==(const Quaternion &p_quaternion) const;
+	constexpr bool operator!=(const Quaternion &p_quaternion) const;
+
+	explicit operator String() const;
+
+	constexpr Quaternion() :
+			x(0), y(0), z(0), w(1) {}
+
+	constexpr Quaternion(real_t p_x, real_t p_y, real_t p_z, real_t p_w) :
+			x(p_x), y(p_y), z(p_z), w(p_w) {}
+
+	Quaternion(const Vector3 &p_axis, real_t p_angle);
+
+	constexpr Quaternion(const Quaternion &p_q) :
+			x(p_q.x), y(p_q.y), z(p_q.z), w(p_q.w) {}
+
+	constexpr void operator=(const Quaternion &p_q) {
+		x = p_q.x;
+		y = p_q.y;
+		z = p_q.z;
+		w = p_q.w;
+	}
+
+	Quaternion(const Vector3 &p_v0, const Vector3 &p_v1) { // Shortest arc.
+#ifdef MATH_CHECKS
+		ERR_FAIL_COND_MSG(p_v0.is_zero_approx() || p_v1.is_zero_approx(), "The vectors must not be zero.");
+#endif
+#ifdef REAL_T_IS_DOUBLE
+		constexpr real_t ALMOST_ONE = 0.999999999999999;
+#else
+		constexpr real_t ALMOST_ONE = 0.99999975f;
+#endif
+		Vector3 n0 = p_v0.normalized();
+		Vector3 n1 = p_v1.normalized();
+		real_t d = n0.dot(n1);
+		if (Math::abs(d) > ALMOST_ONE) {
+			if (d >= 0) {
+				return; // Vectors are same.
+			}
+			Vector3 axis = n0.get_any_perpendicular();
+			x = axis.x;
+			y = axis.y;
+			z = axis.z;
+			w = 0;
+		} else {
+			Vector3 c = n0.cross(n1);
+			real_t s = Math::sqrt((1.0f + d) * 2.0f);
+			real_t rs = 1.0f / s;
+
+			x = c.x * rs;
+			y = c.y * rs;
+			z = c.z * rs;
+			w = s * 0.5f;
+		}
+		normalize();
+	}
+};
+
+real_t Quaternion::dot(const Quaternion &p_q) const {
+	return x * p_q.x + y * p_q.y + z * p_q.z + w * p_q.w;
+}
+
+real_t Quaternion::length_squared() const {
+	return dot(*this);
+}
+
+constexpr void Quaternion::operator+=(const Quaternion &p_q) {
+	x += p_q.x;
+	y += p_q.y;
+	z += p_q.z;
+	w += p_q.w;
+}
+
+constexpr void Quaternion::operator-=(const Quaternion &p_q) {
+	x -= p_q.x;
+	y -= p_q.y;
+	z -= p_q.z;
+	w -= p_q.w;
+}
+
+constexpr void Quaternion::operator*=(real_t p_s) {
+	x *= p_s;
+	y *= p_s;
+	z *= p_s;
+	w *= p_s;
+}
+
+constexpr void Quaternion::operator/=(real_t p_s) {
+	*this *= (1 / p_s);
+}
+
+constexpr Quaternion Quaternion::operator+(const Quaternion &p_q2) const {
+	const Quaternion &q1 = *this;
+	return Quaternion(q1.x + p_q2.x, q1.y + p_q2.y, q1.z + p_q2.z, q1.w + p_q2.w);
+}
+
+constexpr Quaternion Quaternion::operator-(const Quaternion &p_q2) const {
+	const Quaternion &q1 = *this;
+	return Quaternion(q1.x - p_q2.x, q1.y - p_q2.y, q1.z - p_q2.z, q1.w - p_q2.w);
+}
+
+constexpr Quaternion Quaternion::operator-() const {
+	const Quaternion &q2 = *this;
+	return Quaternion(-q2.x, -q2.y, -q2.z, -q2.w);
+}
+
+constexpr Quaternion Quaternion::operator*(real_t p_s) const {
+	return Quaternion(x * p_s, y * p_s, z * p_s, w * p_s);
+}
+
+constexpr Quaternion Quaternion::operator/(real_t p_s) const {
+	return *this * (1 / p_s);
+}
+
+constexpr bool Quaternion::operator==(const Quaternion &p_quaternion) const {
+	return x == p_quaternion.x && y == p_quaternion.y && z == p_quaternion.z && w == p_quaternion.w;
+}
+
+constexpr bool Quaternion::operator!=(const Quaternion &p_quaternion) const {
+	return x != p_quaternion.x || y != p_quaternion.y || z != p_quaternion.z || w != p_quaternion.w;
+}
+
+constexpr void Quaternion::operator*=(const Quaternion &p_q) {
+	real_t xx = w * p_q.x + x * p_q.w + y * p_q.z - z * p_q.y;
+	real_t yy = w * p_q.y + y * p_q.w + z * p_q.x - x * p_q.z;
+	real_t zz = w * p_q.z + z * p_q.w + x * p_q.y - y * p_q.x;
+	w = w * p_q.w - x * p_q.x - y * p_q.y - z * p_q.z;
+	x = xx;
+	y = yy;
+	z = zz;
+}
+
+constexpr Quaternion Quaternion::operator*(const Quaternion &p_q) const {
+	Quaternion r = *this;
+	r *= p_q;
+	return r;
+}
+
+constexpr Quaternion operator*(real_t p_real, const Quaternion &p_quaternion) {
+	return p_quaternion * p_real;
+}

core/math/quick_hull.cpp

@@ -0,0 +1,463 @@
+/**************************************************************************/
+/*  quick_hull.cpp                                                        */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "quick_hull.h"
+
+#include "core/templates/hash_map.h"
+#include "core/templates/hash_set.h"
+
+uint32_t QuickHull::debug_stop_after = 0xFFFFFFFF;
+
+Error QuickHull::build(const Vector<Vector3> &p_points, Geometry3D::MeshData &r_mesh) {
+	/* CREATE AABB VOLUME */
+
+	AABB aabb;
+	for (int i = 0; i < p_points.size(); i++) {
+		if (i == 0) {
+			aabb.position = p_points[i];
+		} else {
+			aabb.expand_to(p_points[i]);
+		}
+	}
+
+	if (aabb.size == Vector3()) {
+		return ERR_CANT_CREATE;
+	}
+
+	Vector<bool> valid_points;
+	valid_points.resize(p_points.size());
+	HashSet<Vector3> valid_cache;
+
+	for (int i = 0; i < p_points.size(); i++) {
+		Vector3 sp = p_points[i].snappedf(0.0001);
+		if (valid_cache.has(sp)) {
+			valid_points.write[i] = false;
+		} else {
+			valid_points.write[i] = true;
+			valid_cache.insert(sp);
+		}
+	}
+
+	/* CREATE INITIAL SIMPLEX */
+
+	int longest_axis = aabb.get_longest_axis_index();
+
+	//first two vertices are the most distant
+	int simplex[4] = { 0 };
+
+	{
+		real_t max = 0, min = 0;
+
+		for (int i = 0; i < p_points.size(); i++) {
+			if (!valid_points[i]) {
+				continue;
+			}
+			real_t d = p_points[i][longest_axis];
+			if (i == 0 || d < min) {
+				simplex[0] = i;
+				min = d;
+			}
+
+			if (i == 0 || d > max) {
+				simplex[1] = i;
+				max = d;
+			}
+		}
+	}
+
+	//third vertex is one most further away from the line
+
+	{
+		real_t maxd = 0;
+		Vector3 rel12 = p_points[simplex[0]] - p_points[simplex[1]];
+
+		for (int i = 0; i < p_points.size(); i++) {
+			if (!valid_points[i]) {
+				continue;
+			}
+
+			Vector3 n = rel12.cross(p_points[simplex[0]] - p_points[i]).cross(rel12).normalized();
+			real_t d = Math::abs(n.dot(p_points[simplex[0]]) - n.dot(p_points[i]));
+
+			if (i == 0 || d > maxd) {
+				maxd = d;
+				simplex[2] = i;
+			}
+		}
+	}
+
+	//fourth vertex is the one most further away from the plane
+
+	{
+		real_t maxd = 0;
+		Plane p(p_points[simplex[0]], p_points[simplex[1]], p_points[simplex[2]]);
+
+		for (int i = 0; i < p_points.size(); i++) {
+			if (!valid_points[i]) {
+				continue;
+			}
+
+			real_t d = Math::abs(p.distance_to(p_points[i]));
+
+			if (i == 0 || d > maxd) {
+				maxd = d;
+				simplex[3] = i;
+			}
+		}
+	}
+
+	//compute center of simplex, this is a point always warranted to be inside
+	Vector3 center;
+
+	for (int i = 0; i < 4; i++) {
+		center += p_points[simplex[i]];
+	}
+
+	center /= 4.0;
+
+	//add faces
+
+	List<Face> faces;
+
+	for (int i = 0; i < 4; i++) {
+		static const int face_order[4][3] = {
+			{ 0, 1, 2 },
+			{ 0, 1, 3 },
+			{ 0, 2, 3 },
+			{ 1, 2, 3 }
+		};
+
+		Face f;
+		for (int j = 0; j < 3; j++) {
+			f.vertices[j] = simplex[face_order[i][j]];
+		}
+
+		Plane p(p_points[f.vertices[0]], p_points[f.vertices[1]], p_points[f.vertices[2]]);
+
+		if (p.is_point_over(center)) {
+			//flip face to clockwise if facing inwards
+			SWAP(f.vertices[0], f.vertices[1]);
+			p = -p;
+		}
+
+		f.plane = p;
+
+		faces.push_back(f);
+	}
+
+	real_t over_tolerance = 3 * UNIT_EPSILON * (aabb.size.x + aabb.size.y + aabb.size.z);
+
+	/* COMPUTE AVAILABLE VERTICES */
+
+	for (int i = 0; i < p_points.size(); i++) {
+		if (i == simplex[0]) {
+			continue;
+		}
+		if (i == simplex[1]) {
+			continue;
+		}
+		if (i == simplex[2]) {
+			continue;
+		}
+		if (i == simplex[3]) {
+			continue;
+		}
+		if (!valid_points[i]) {
+			continue;
+		}
+
+		for (Face &E : faces) {
+			if (E.plane.distance_to(p_points[i]) > over_tolerance) {
+				E.points_over.push_back(i);
+				break;
+			}
+		}
+	}
+
+	faces.sort(); // sort them, so the ones with points are in the back
+
+	/* BUILD HULL */
+
+	//poop face (while still remain)
+	//find further away point
+	//find lit faces
+	//determine horizon edges
+	//build new faces with horizon edges, them assign points side from all lit faces
+	//remove lit faces
+
+	uint32_t debug_stop = debug_stop_after;
+
+	while (debug_stop > 0 && faces.back()->get().points_over.size()) {
+		debug_stop--;
+		Face &f = faces.back()->get();
+
+		//find vertex most outside
+		int next = -1;
+		real_t next_d = 0;
+
+		for (int i = 0; i < f.points_over.size(); i++) {
+			real_t d = f.plane.distance_to(p_points[f.points_over[i]]);
+
+			if (d > next_d) {
+				next_d = d;
+				next = i;
+			}
+		}
+
+		ERR_FAIL_COND_V(next == -1, ERR_BUG);
+
+		Vector3 v = p_points[f.points_over[next]];
+
+		//find lit faces and lit edges
+		List<List<Face>::Element *> lit_faces; //lit face is a death sentence
+
+		HashMap<Edge, FaceConnect, Edge> lit_edges; //create this on the flight, should not be that bad for performance and simplifies code a lot
+
+		for (List<Face>::Element *E = faces.front(); E; E = E->next()) {
+			if (E->get().plane.distance_to(v) > 0) {
+				lit_faces.push_back(E);
+
+				for (int i = 0; i < 3; i++) {
+					uint32_t a = E->get().vertices[i];
+					uint32_t b = E->get().vertices[(i + 1) % 3];
+					Edge e(a, b);
+
+					HashMap<Edge, FaceConnect, Edge>::Iterator F = lit_edges.find(e);
+					if (!F) {
+						F = lit_edges.insert(e, FaceConnect());
+					}
+					if (e.vertices[0] == a) {
+						//left
+						F->value.left = E;
+					} else {
+						F->value.right = E;
+					}
+				}
+			}
+		}
+
+		//create new faces from horizon edges
+		List<List<Face>::Element *> new_faces; //new faces
+
+		for (KeyValue<Edge, FaceConnect> &E : lit_edges) {
+			FaceConnect &fc = E.value;
+			if (fc.left && fc.right) {
+				continue; //edge is uninteresting, not on horizon
+			}
+
+			//create new face!
+
+			Face face;
+			face.vertices[0] = f.points_over[next];
+			face.vertices[1] = E.key.vertices[0];
+			face.vertices[2] = E.key.vertices[1];
+
+			Plane p(p_points[face.vertices[0]], p_points[face.vertices[1]], p_points[face.vertices[2]]);
+
+			if (p.is_point_over(center)) {
+				//flip face to clockwise if facing inwards
+				SWAP(face.vertices[0], face.vertices[1]);
+				p = -p;
+			}
+
+			face.plane = p;
+			new_faces.push_back(faces.push_back(face));
+		}
+
+		//distribute points into new faces
+
+		for (List<Face>::Element *&F : lit_faces) {
+			Face &lf = F->get();
+
+			for (int i = 0; i < lf.points_over.size(); i++) {
+				if (lf.points_over[i] == f.points_over[next]) { //do not add current one
+					continue;
+				}
+
+				Vector3 p = p_points[lf.points_over[i]];
+				for (List<Face>::Element *&E : new_faces) {
+					Face &f2 = E->get();
+					if (f2.plane.distance_to(p) > over_tolerance) {
+						f2.points_over.push_back(lf.points_over[i]);
+						break;
+					}
+				}
+			}
+		}
+
+		//erase lit faces
+
+		while (lit_faces.size()) {
+			faces.erase(lit_faces.front()->get());
+			lit_faces.pop_front();
+		}
+
+		//put faces that contain no points on the front
+
+		for (List<Face>::Element *&E : new_faces) {
+			Face &f2 = E->get();
+			if (f2.points_over.is_empty()) {
+				faces.move_to_front(E);
+			}
+		}
+
+		//whew, done with iteration, go next
+	}
+
+	/* CREATE MESHDATA */
+
+	//make a map of edges again
+	HashMap<Edge, RetFaceConnect, Edge> ret_edges;
+	List<Geometry3D::MeshData::Face> ret_faces;
+
+	for (const Face &E : faces) {
+		Geometry3D::MeshData::Face f;
+		f.plane = E.plane;
+
+		for (int i = 0; i < 3; i++) {
+			f.indices.push_back(E.vertices[i]);
+		}
+
+		List<Geometry3D::MeshData::Face>::Element *F = ret_faces.push_back(f);
+
+		for (int i = 0; i < 3; i++) {
+			uint32_t a = E.vertices[i];
+			uint32_t b = E.vertices[(i + 1) % 3];
+			Edge e(a, b);
+
+			HashMap<Edge, RetFaceConnect, Edge>::Iterator G = ret_edges.find(e);
+			if (!G) {
+				G = ret_edges.insert(e, RetFaceConnect());
+			}
+			if (e.vertices[0] == a) {
+				//left
+				G->value.left = F;
+			} else {
+				G->value.right = F;
+			}
+		}
+	}
+
+	//fill faces
+
+	for (List<Geometry3D::MeshData::Face>::Element *E = ret_faces.front(); E; E = E->next()) {
+		Geometry3D::MeshData::Face &f = E->get();
+
+		for (uint32_t i = 0; i < f.indices.size(); i++) {
+			int a = E->get().indices[i];
+			int b = E->get().indices[(i + 1) % f.indices.size()];
+			Edge e(a, b);
+
+			HashMap<Edge, RetFaceConnect, Edge>::Iterator F = ret_edges.find(e);
+
+			ERR_CONTINUE(!F);
+			List<Geometry3D::MeshData::Face>::Element *O = F->value.left == E ? F->value.right : F->value.left;
+			ERR_CONTINUE(O == E);
+			ERR_CONTINUE(O == nullptr);
+
+			if (O->get().plane.is_equal_approx(f.plane)) {
+				//merge and delete edge and contiguous face, while repointing edges (uuugh!)
+				int o_index_size = O->get().indices.size();
+
+				for (int j = 0; j < o_index_size; j++) {
+					//search a
+					if (O->get().indices[j] == a) {
+						//append the rest
+						for (int k = 0; k < o_index_size; k++) {
+							int idx = O->get().indices[(k + j) % o_index_size];
+							int idxn = O->get().indices[(k + j + 1) % o_index_size];
+							if (idx == b && idxn == a) { //already have b!
+								break;
+							}
+							if (idx != a) {
+								f.indices.insert(i + 1, idx);
+								i++;
+							}
+							Edge e2(idx, idxn);
+
+							HashMap<Edge, RetFaceConnect, Edge>::Iterator F2 = ret_edges.find(e2);
+							ERR_CONTINUE(!F2);
+							//change faceconnect, point to this face instead
+							if (F2->value.left == O) {
+								F2->value.left = E;
+							} else if (F2->value.right == O) {
+								F2->value.right = E;
+							}
+						}
+
+						break;
+					}
+				}
+
+				// remove all edge connections to this face
+				for (KeyValue<Edge, RetFaceConnect> &G : ret_edges) {
+					if (G.value.left == O) {
+						G.value.left = nullptr;
+					}
+
+					if (G.value.right == O) {
+						G.value.right = nullptr;
+					}
+				}
+
+				ret_edges.remove(F); //remove the edge
+				ret_faces.erase(O); //remove the face
+			}
+		}
+	}
+
+	//fill mesh
+	r_mesh.faces.clear();
+	r_mesh.faces.resize(ret_faces.size());
+
+	HashMap<List<Geometry3D::MeshData::Face>::Element *, int> face_indices;
+
+	int idx = 0;
+	for (List<Geometry3D::MeshData::Face>::Element *E = ret_faces.front(); E; E = E->next()) {
+		face_indices[E] = idx;
+		r_mesh.faces[idx++] = E->get();
+	}
+	r_mesh.edges.resize(ret_edges.size());
+	idx = 0;
+	for (const KeyValue<Edge, RetFaceConnect> &E : ret_edges) {
+		Geometry3D::MeshData::Edge e;
+		e.vertex_a = E.key.vertices[0];
+		e.vertex_b = E.key.vertices[1];
+		ERR_CONTINUE(!face_indices.has(E.value.left));
+		ERR_CONTINUE(!face_indices.has(E.value.right));
+		e.face_a = face_indices[E.value.left];
+		e.face_b = face_indices[E.value.right];
+		r_mesh.edges[idx++] = e;
+	}
+
+	r_mesh.vertices = p_points;
+
+	return OK;
+}

core/math/quick_hull.h

@@ -0,0 +1,90 @@
+/**************************************************************************/
+/*  quick_hull.h                                                          */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "core/math/geometry_3d.h"
+#include "core/templates/list.h"
+
+class QuickHull {
+public:
+	struct Edge {
+		union {
+			uint32_t vertices[2];
+			uint64_t id = 0;
+		};
+
+		static uint32_t hash(const Edge &p_edge) {
+			return hash_one_uint64(p_edge.id);
+		}
+
+		bool operator<(const Edge &p_edge) const {
+			return id < p_edge.id;
+		}
+		bool operator==(const Edge &p_edge) const {
+			return id == p_edge.id;
+		}
+
+		Edge(int p_vtx_a = 0, int p_vtx_b = 0) {
+			if (p_vtx_a > p_vtx_b) {
+				SWAP(p_vtx_a, p_vtx_b);
+			}
+
+			vertices[0] = p_vtx_a;
+			vertices[1] = p_vtx_b;
+		}
+	};
+
+	struct Face {
+		Plane plane;
+		uint32_t vertices[3] = { 0 };
+		Vector<int> points_over;
+
+		bool operator<(const Face &p_face) const {
+			return points_over.size() < p_face.points_over.size();
+		}
+	};
+
+private:
+	struct FaceConnect {
+		List<Face>::Element *left = nullptr;
+		List<Face>::Element *right = nullptr;
+		FaceConnect() {}
+	};
+	struct RetFaceConnect {
+		List<Geometry3D::MeshData::Face>::Element *left = nullptr;
+		List<Geometry3D::MeshData::Face>::Element *right = nullptr;
+		RetFaceConnect() {}
+	};
+
+public:
+	static uint32_t debug_stop_after;
+	static Error build(const Vector<Vector3> &p_points, Geometry3D::MeshData &r_mesh);
+};

core/math/random_number_generator.cpp

@@ -0,0 +1,53 @@
+/**************************************************************************/
+/*  random_number_generator.cpp                                           */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "random_number_generator.h"
+
+void RandomNumberGenerator::_bind_methods() {
+	ClassDB::bind_method(D_METHOD("set_seed", "seed"), &RandomNumberGenerator::set_seed);
+	ClassDB::bind_method(D_METHOD("get_seed"), &RandomNumberGenerator::get_seed);
+
+	ClassDB::bind_method(D_METHOD("set_state", "state"), &RandomNumberGenerator::set_state);
+	ClassDB::bind_method(D_METHOD("get_state"), &RandomNumberGenerator::get_state);
+
+	ClassDB::bind_method(D_METHOD("randi"), &RandomNumberGenerator::randi);
+	ClassDB::bind_method(D_METHOD("randf"), &RandomNumberGenerator::randf);
+	ClassDB::bind_method(D_METHOD("randfn", "mean", "deviation"), &RandomNumberGenerator::randfn, DEFVAL(0.0), DEFVAL(1.0));
+	ClassDB::bind_method(D_METHOD("randf_range", "from", "to"), &RandomNumberGenerator::randf_range);
+	ClassDB::bind_method(D_METHOD("randi_range", "from", "to"), &RandomNumberGenerator::randi_range);
+	ClassDB::bind_method(D_METHOD("rand_weighted", "weights"), &RandomNumberGenerator::rand_weighted);
+	ClassDB::bind_method(D_METHOD("randomize"), &RandomNumberGenerator::randomize);
+
+	ADD_PROPERTY(PropertyInfo(Variant::INT, "seed"), "set_seed", "get_seed");
+	ADD_PROPERTY(PropertyInfo(Variant::INT, "state"), "set_state", "get_state");
+	// Default values are non-deterministic, override for doc generation purposes.
+	ADD_PROPERTY_DEFAULT("seed", 0);
+	ADD_PROPERTY_DEFAULT("state", 0);
+}

core/math/random_number_generator.h

@@ -0,0 +1,62 @@
+/**************************************************************************/
+/*  random_number_generator.h                                             */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "core/math/random_pcg.h"
+#include "core/object/ref_counted.h"
+
+class RandomNumberGenerator : public RefCounted {
+	GDCLASS(RandomNumberGenerator, RefCounted);
+
+protected:
+	RandomPCG randbase;
+
+	static void _bind_methods();
+
+public:
+	_FORCE_INLINE_ void set_seed(uint64_t p_seed) { randbase.seed(p_seed); }
+	_FORCE_INLINE_ uint64_t get_seed() { return randbase.get_seed(); }
+
+	_FORCE_INLINE_ void set_state(uint64_t p_state) { randbase.set_state(p_state); }
+	_FORCE_INLINE_ uint64_t get_state() const { return randbase.get_state(); }
+
+	_FORCE_INLINE_ void randomize() { randbase.randomize(); }
+
+	_FORCE_INLINE_ uint32_t randi() { return randbase.rand(); }
+	_FORCE_INLINE_ real_t randf() { return randbase.randf(); }
+	_FORCE_INLINE_ real_t randf_range(real_t p_from, real_t p_to) { return randbase.random(p_from, p_to); }
+	_FORCE_INLINE_ real_t randfn(real_t p_mean = 0.0, real_t p_deviation = 1.0) { return randbase.randfn(p_mean, p_deviation); }
+	_FORCE_INLINE_ int randi_range(int p_from, int p_to) { return randbase.random(p_from, p_to); }
+
+	_FORCE_INLINE_ int64_t rand_weighted(const Vector<float> &p_weights) { return randbase.rand_weighted(p_weights); }
+
+	RandomNumberGenerator() { randbase.randomize(); }
+};

core/math/random_pcg.cpp

@@ -0,0 +1,94 @@
+/**************************************************************************/
+/*  random_pcg.cpp                                                        */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "random_pcg.h"
+
+#include "core/os/os.h"
+#include "core/templates/vector.h"
+
+RandomPCG::RandomPCG(uint64_t p_seed, uint64_t p_inc) :
+		pcg(),
+		current_inc(p_inc) {
+	seed(p_seed);
+}
+
+void RandomPCG::randomize() {
+	seed(((uint64_t)OS::get_singleton()->get_unix_time() + OS::get_singleton()->get_ticks_usec()) * pcg.state + PCG_DEFAULT_INC_64);
+}
+
+int64_t RandomPCG::rand_weighted(const Vector<float> &p_weights) {
+	ERR_FAIL_COND_V_MSG(p_weights.is_empty(), -1, "Weights array is empty.");
+	int64_t weights_size = p_weights.size();
+	const float *weights = p_weights.ptr();
+	float weights_sum = 0.0;
+	for (int64_t i = 0; i < weights_size; ++i) {
+		weights_sum += weights[i];
+	}
+
+	float remaining_distance = randf() * weights_sum;
+	for (int64_t i = 0; i < weights_size; ++i) {
+		remaining_distance -= weights[i];
+		if (remaining_distance < 0) {
+			return i;
+		}
+	}
+
+	for (int64_t i = weights_size - 1; i >= 0; --i) {
+		if (weights[i] > 0) {
+			return i;
+		}
+	}
+	return -1;
+}
+
+double RandomPCG::random(double p_from, double p_to) {
+	return randd() * (p_to - p_from) + p_from;
+}
+
+float RandomPCG::random(float p_from, float p_to) {
+	return randf() * (p_to - p_from) + p_from;
+}
+
+int RandomPCG::random(int p_from, int p_to) {
+	if (p_from == p_to) {
+		return p_from;
+	}
+
+	int64_t min = MIN(p_from, p_to);
+	int64_t max = MAX(p_from, p_to);
+	uint32_t diff = static_cast<uint32_t>(max - min);
+
+	if (diff == UINT32_MAX) {
+		// Can't add 1 to max uint32_t value for inclusive range, so call rand without passing bounds.
+		return static_cast<int64_t>(rand()) + min;
+	}
+
+	return static_cast<int64_t>(rand(diff + 1U)) + min;
+}

core/math/random_pcg.h

@@ -0,0 +1,138 @@
+/**************************************************************************/
+/*  random_pcg.h                                                          */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "core/math/math_funcs.h"
+
+#include "thirdparty/misc/pcg.h"
+
+#if defined(__GNUC__)
+#define CLZ32(x) __builtin_clz(x)
+#elif defined(_MSC_VER)
+#include <intrin.h>
+static int __bsr_clz32(uint32_t x) {
+	unsigned long index;
+	_BitScanReverse(&index, x);
+	return 31 - index;
+}
+#define CLZ32(x) __bsr_clz32(x)
+#endif
+
+template <typename T>
+class Vector;
+
+class RandomPCG {
+	pcg32_random_t pcg;
+	uint64_t current_seed = 0; // The seed the current generator state started from.
+	uint64_t current_inc = 0;
+
+public:
+	static const uint64_t DEFAULT_SEED = 12047754176567800795U;
+	static const uint64_t DEFAULT_INC = PCG_DEFAULT_INC_64;
+
+	RandomPCG(uint64_t p_seed = DEFAULT_SEED, uint64_t p_inc = DEFAULT_INC);
+
+	_FORCE_INLINE_ void seed(uint64_t p_seed) {
+		current_seed = p_seed;
+		pcg32_srandom_r(&pcg, current_seed, current_inc);
+	}
+	_FORCE_INLINE_ uint64_t get_seed() { return current_seed; }
+
+	_FORCE_INLINE_ void set_state(uint64_t p_state) { pcg.state = p_state; }
+	_FORCE_INLINE_ uint64_t get_state() const { return pcg.state; }
+
+	void randomize();
+	_FORCE_INLINE_ uint32_t rand() {
+		return pcg32_random_r(&pcg);
+	}
+	_FORCE_INLINE_ uint32_t rand(uint32_t bounds) {
+		return pcg32_boundedrand_r(&pcg, bounds);
+	}
+
+	int64_t rand_weighted(const Vector<float> &p_weights);
+
+	// Obtaining floating point numbers in [0, 1] range with "good enough" uniformity.
+	// These functions sample the output of rand() as the fraction part of an infinite binary number,
+	// with some tricks applied to reduce ops and branching:
+	// 1. Instead of shifting to the first 1 and connecting random bits, we simply set the MSB and LSB to 1.
+	//    Provided that the RNG is actually uniform bit by bit, this should have the exact same effect.
+	// 2. In order to compensate for exponent info loss, we count zeros from another random number,
+	//    and just add that to the initial offset.
+	//    This has the same probability as counting and shifting an actual bit stream: 2^-n for n zeroes.
+	// For all numbers above 2^-96 (2^-64 for floats), the functions should be uniform.
+	// However, all numbers below that threshold are floored to 0.
+	// The thresholds are chosen to minimize rand() calls while keeping the numbers within a totally subjective quality standard.
+	// If clz or ldexp isn't available, fall back to bit truncation for performance, sacrificing uniformity.
+	_FORCE_INLINE_ double randd() {
+#if defined(CLZ32)
+		uint32_t proto_exp_offset = rand();
+		if (unlikely(proto_exp_offset == 0)) {
+			return 0;
+		}
+		uint64_t significand = (((uint64_t)rand()) << 32) | rand() | 0x8000000000000001U;
+		return std::ldexp((double)significand, -64 - CLZ32(proto_exp_offset));
+#else
+#pragma message("RandomPCG::randd - intrinsic clz is not available, falling back to bit truncation")
+		return (double)(((((uint64_t)rand()) << 32) | rand()) & 0x1FFFFFFFFFFFFFU) / (double)0x1FFFFFFFFFFFFFU;
+#endif
+	}
+	_FORCE_INLINE_ float randf() {
+#if defined(CLZ32)
+		uint32_t proto_exp_offset = rand();
+		if (unlikely(proto_exp_offset == 0)) {
+			return 0;
+		}
+		return std::ldexp((float)(rand() | 0x80000001), -32 - CLZ32(proto_exp_offset));
+#else
+#pragma message("RandomPCG::randf - intrinsic clz is not available, falling back to bit truncation")
+		return (float)(rand() & 0xFFFFFF) / (float)0xFFFFFF;
+#endif
+	}
+
+	_FORCE_INLINE_ double randfn(double p_mean, double p_deviation) {
+		double temp = randd();
+		if (temp < CMP_EPSILON) {
+			temp += CMP_EPSILON; // To prevent generating of INF value in log function, resulting to return NaN value from this function.
+		}
+		return p_mean + p_deviation * (std::cos(Math::TAU * randd()) * std::sqrt(-2.0 * std::log(temp))); // Box-Muller transform.
+	}
+	_FORCE_INLINE_ float randfn(float p_mean, float p_deviation) {
+		float temp = randf();
+		if (temp < CMP_EPSILON) {
+			temp += CMP_EPSILON; // To prevent generating of INF value in log function, resulting to return NaN value from this function.
+		}
+		return p_mean + p_deviation * (std::cos((float)Math::TAU * randf()) * std::sqrt(-2.0 * std::log(temp))); // Box-Muller transform.
+	}
+
+	double random(double p_from, double p_to);
+	float random(float p_from, float p_to);
+	int random(int p_from, int p_to);
+};

core/math/rect2.cpp

@@ -0,0 +1,295 @@
+/**************************************************************************/
+/*  rect2.cpp                                                             */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "rect2.h"
+
+#include "core/math/rect2i.h"
+#include "core/math/transform_2d.h"
+#include "core/string/ustring.h"
+
+bool Rect2::is_equal_approx(const Rect2 &p_rect) const {
+	return position.is_equal_approx(p_rect.position) && size.is_equal_approx(p_rect.size);
+}
+
+bool Rect2::is_same(const Rect2 &p_rect) const {
+	return position.is_same(p_rect.position) && size.is_same(p_rect.size);
+}
+
+bool Rect2::is_finite() const {
+	return position.is_finite() && size.is_finite();
+}
+
+bool Rect2::intersects_segment(const Point2 &p_from, const Point2 &p_to, Point2 *r_pos, Point2 *r_normal) const {
+#ifdef MATH_CHECKS
+	if (unlikely(size.x < 0 || size.y < 0)) {
+		ERR_PRINT("Rect2 size is negative, this is not supported. Use Rect2.abs() to get a Rect2 with a positive size.");
+	}
+#endif
+	real_t min = 0, max = 1;
+	int axis = 0;
+	real_t sign = 0;
+
+	for (int i = 0; i < 2; i++) {
+		real_t seg_from = p_from[i];
+		real_t seg_to = p_to[i];
+		real_t box_begin = position[i];
+		real_t box_end = box_begin + size[i];
+		real_t cmin, cmax;
+		real_t csign;
+
+		if (seg_from < seg_to) {
+			if (seg_from > box_end || seg_to < box_begin) {
+				return false;
+			}
+			real_t length = seg_to - seg_from;
+			cmin = (seg_from < box_begin) ? ((box_begin - seg_from) / length) : 0;
+			cmax = (seg_to > box_end) ? ((box_end - seg_from) / length) : 1;
+			csign = -1.0;
+
+		} else {
+			if (seg_to > box_end || seg_from < box_begin) {
+				return false;
+			}
+			real_t length = seg_to - seg_from;
+			cmin = (seg_from > box_end) ? (box_end - seg_from) / length : 0;
+			cmax = (seg_to < box_begin) ? (box_begin - seg_from) / length : 1;
+			csign = 1.0;
+		}
+
+		if (cmin > min) {
+			min = cmin;
+			axis = i;
+			sign = csign;
+		}
+		if (cmax < max) {
+			max = cmax;
+		}
+		if (max < min) {
+			return false;
+		}
+	}
+
+	Vector2 rel = p_to - p_from;
+
+	if (r_normal) {
+		Vector2 normal;
+		normal[axis] = sign;
+		*r_normal = normal;
+	}
+
+	if (r_pos) {
+		*r_pos = p_from + rel * min;
+	}
+
+	return true;
+}
+
+bool Rect2::intersects_transformed(const Transform2D &p_xform, const Rect2 &p_rect) const {
+#ifdef MATH_CHECKS
+	if (unlikely(size.x < 0 || size.y < 0 || p_rect.size.x < 0 || p_rect.size.y < 0)) {
+		ERR_PRINT("Rect2 size is negative, this is not supported. Use Rect2.abs() to get a Rect2 with a positive size.");
+	}
+#endif
+	//SAT intersection between local and transformed rect2
+
+	Vector2 xf_points[4] = {
+		p_xform.xform(p_rect.position),
+		p_xform.xform(Vector2(p_rect.position.x + p_rect.size.x, p_rect.position.y)),
+		p_xform.xform(Vector2(p_rect.position.x, p_rect.position.y + p_rect.size.y)),
+		p_xform.xform(Vector2(p_rect.position.x + p_rect.size.x, p_rect.position.y + p_rect.size.y)),
+	};
+
+	real_t low_limit;
+
+	//base rect2 first (faster)
+
+	if (xf_points[0].y > position.y) {
+		goto next1;
+	}
+	if (xf_points[1].y > position.y) {
+		goto next1;
+	}
+	if (xf_points[2].y > position.y) {
+		goto next1;
+	}
+	if (xf_points[3].y > position.y) {
+		goto next1;
+	}
+
+	return false;
+
+next1:
+
+	low_limit = position.y + size.y;
+
+	if (xf_points[0].y < low_limit) {
+		goto next2;
+	}
+	if (xf_points[1].y < low_limit) {
+		goto next2;
+	}
+	if (xf_points[2].y < low_limit) {
+		goto next2;
+	}
+	if (xf_points[3].y < low_limit) {
+		goto next2;
+	}
+
+	return false;
+
+next2:
+
+	if (xf_points[0].x > position.x) {
+		goto next3;
+	}
+	if (xf_points[1].x > position.x) {
+		goto next3;
+	}
+	if (xf_points[2].x > position.x) {
+		goto next3;
+	}
+	if (xf_points[3].x > position.x) {
+		goto next3;
+	}
+
+	return false;
+
+next3:
+
+	low_limit = position.x + size.x;
+
+	if (xf_points[0].x < low_limit) {
+		goto next4;
+	}
+	if (xf_points[1].x < low_limit) {
+		goto next4;
+	}
+	if (xf_points[2].x < low_limit) {
+		goto next4;
+	}
+	if (xf_points[3].x < low_limit) {
+		goto next4;
+	}
+
+	return false;
+
+next4:
+
+	Vector2 xf_points2[4] = {
+		position,
+		Vector2(position.x + size.x, position.y),
+		Vector2(position.x, position.y + size.y),
+		Vector2(position.x + size.x, position.y + size.y),
+	};
+
+	real_t maxa = p_xform.columns[0].dot(xf_points2[0]);
+	real_t mina = maxa;
+
+	real_t dp = p_xform.columns[0].dot(xf_points2[1]);
+	maxa = MAX(dp, maxa);
+	mina = MIN(dp, mina);
+
+	dp = p_xform.columns[0].dot(xf_points2[2]);
+	maxa = MAX(dp, maxa);
+	mina = MIN(dp, mina);
+
+	dp = p_xform.columns[0].dot(xf_points2[3]);
+	maxa = MAX(dp, maxa);
+	mina = MIN(dp, mina);
+
+	real_t maxb = p_xform.columns[0].dot(xf_points[0]);
+	real_t minb = maxb;
+
+	dp = p_xform.columns[0].dot(xf_points[1]);
+	maxb = MAX(dp, maxb);
+	minb = MIN(dp, minb);
+
+	dp = p_xform.columns[0].dot(xf_points[2]);
+	maxb = MAX(dp, maxb);
+	minb = MIN(dp, minb);
+
+	dp = p_xform.columns[0].dot(xf_points[3]);
+	maxb = MAX(dp, maxb);
+	minb = MIN(dp, minb);
+
+	if (mina > maxb) {
+		return false;
+	}
+	if (minb > maxa) {
+		return false;
+	}
+
+	maxa = p_xform.columns[1].dot(xf_points2[0]);
+	mina = maxa;
+
+	dp = p_xform.columns[1].dot(xf_points2[1]);
+	maxa = MAX(dp, maxa);
+	mina = MIN(dp, mina);
+
+	dp = p_xform.columns[1].dot(xf_points2[2]);
+	maxa = MAX(dp, maxa);
+	mina = MIN(dp, mina);
+
+	dp = p_xform.columns[1].dot(xf_points2[3]);
+	maxa = MAX(dp, maxa);
+	mina = MIN(dp, mina);
+
+	maxb = p_xform.columns[1].dot(xf_points[0]);
+	minb = maxb;
+
+	dp = p_xform.columns[1].dot(xf_points[1]);
+	maxb = MAX(dp, maxb);
+	minb = MIN(dp, minb);
+
+	dp = p_xform.columns[1].dot(xf_points[2]);
+	maxb = MAX(dp, maxb);
+	minb = MIN(dp, minb);
+
+	dp = p_xform.columns[1].dot(xf_points[3]);
+	maxb = MAX(dp, maxb);
+	minb = MIN(dp, minb);
+
+	if (mina > maxb) {
+		return false;
+	}
+	if (minb > maxa) {
+		return false;
+	}
+
+	return true;
+}
+
+Rect2::operator String() const {
+	return "[P: " + position.operator String() + ", S: " + size.operator String() + "]";
+}
+
+Rect2::operator Rect2i() const {
+	return Rect2i(position, size);
+}

core/math/rect2.h

@@ -0,0 +1,376 @@
+/**************************************************************************/
+/*  rect2.h                                                               */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "core/error/error_macros.h"
+#include "core/math/vector2.h"
+
+class String;
+struct Rect2i;
+struct Transform2D;
+
+struct [[nodiscard]] Rect2 {
+	Point2 position;
+	Size2 size;
+
+	const Vector2 &get_position() const { return position; }
+	void set_position(const Vector2 &p_pos) { position = p_pos; }
+	const Vector2 &get_size() const { return size; }
+	void set_size(const Vector2 &p_size) { size = p_size; }
+
+	real_t get_area() const { return size.width * size.height; }
+
+	_FORCE_INLINE_ Vector2 get_center() const { return position + (size * 0.5f); }
+
+	inline bool intersects(const Rect2 &p_rect, bool p_include_borders = false) const {
+#ifdef MATH_CHECKS
+		if (unlikely(size.x < 0 || size.y < 0 || p_rect.size.x < 0 || p_rect.size.y < 0)) {
+			ERR_PRINT("Rect2 size is negative, this is not supported. Use Rect2.abs() to get a Rect2 with a positive size.");
+		}
+#endif
+		if (p_include_borders) {
+			if (position.x > (p_rect.position.x + p_rect.size.width)) {
+				return false;
+			}
+			if ((position.x + size.width) < p_rect.position.x) {
+				return false;
+			}
+			if (position.y > (p_rect.position.y + p_rect.size.height)) {
+				return false;
+			}
+			if ((position.y + size.height) < p_rect.position.y) {
+				return false;
+			}
+		} else {
+			if (position.x >= (p_rect.position.x + p_rect.size.width)) {
+				return false;
+			}
+			if ((position.x + size.width) <= p_rect.position.x) {
+				return false;
+			}
+			if (position.y >= (p_rect.position.y + p_rect.size.height)) {
+				return false;
+			}
+			if ((position.y + size.height) <= p_rect.position.y) {
+				return false;
+			}
+		}
+
+		return true;
+	}
+
+	inline real_t distance_to(const Vector2 &p_point) const {
+#ifdef MATH_CHECKS
+		if (unlikely(size.x < 0 || size.y < 0)) {
+			ERR_PRINT("Rect2 size is negative, this is not supported. Use Rect2.abs() to get a Rect2 with a positive size.");
+		}
+#endif
+		real_t dist = 0.0;
+		bool inside = true;
+
+		if (p_point.x < position.x) {
+			real_t d = position.x - p_point.x;
+			dist = d;
+			inside = false;
+		}
+		if (p_point.y < position.y) {
+			real_t d = position.y - p_point.y;
+			dist = inside ? d : MIN(dist, d);
+			inside = false;
+		}
+		if (p_point.x >= (position.x + size.x)) {
+			real_t d = p_point.x - (position.x + size.x);
+			dist = inside ? d : MIN(dist, d);
+			inside = false;
+		}
+		if (p_point.y >= (position.y + size.y)) {
+			real_t d = p_point.y - (position.y + size.y);
+			dist = inside ? d : MIN(dist, d);
+			inside = false;
+		}
+
+		if (inside) {
+			return 0;
+		} else {
+			return dist;
+		}
+	}
+
+	bool intersects_transformed(const Transform2D &p_xform, const Rect2 &p_rect) const;
+
+	bool intersects_segment(const Point2 &p_from, const Point2 &p_to, Point2 *r_pos = nullptr, Point2 *r_normal = nullptr) const;
+
+	inline bool encloses(const Rect2 &p_rect) const {
+#ifdef MATH_CHECKS
+		if (unlikely(size.x < 0 || size.y < 0 || p_rect.size.x < 0 || p_rect.size.y < 0)) {
+			ERR_PRINT("Rect2 size is negative, this is not supported. Use Rect2.abs() to get a Rect2 with a positive size.");
+		}
+#endif
+		return (p_rect.position.x >= position.x) && (p_rect.position.y >= position.y) &&
+				((p_rect.position.x + p_rect.size.x) <= (position.x + size.x)) &&
+				((p_rect.position.y + p_rect.size.y) <= (position.y + size.y));
+	}
+
+	_FORCE_INLINE_ bool has_area() const {
+		return size.x > 0.0f && size.y > 0.0f;
+	}
+
+	// Returns the intersection between two Rect2s or an empty Rect2 if there is no intersection.
+	inline Rect2 intersection(const Rect2 &p_rect) const {
+		Rect2 new_rect = p_rect;
+
+		if (!intersects(new_rect)) {
+			return Rect2();
+		}
+
+		new_rect.position = p_rect.position.max(position);
+
+		Point2 p_rect_end = p_rect.position + p_rect.size;
+		Point2 end = position + size;
+
+		new_rect.size = p_rect_end.min(end) - new_rect.position;
+
+		return new_rect;
+	}
+
+	inline Rect2 merge(const Rect2 &p_rect) const { ///< return a merged rect
+#ifdef MATH_CHECKS
+		if (unlikely(size.x < 0 || size.y < 0 || p_rect.size.x < 0 || p_rect.size.y < 0)) {
+			ERR_PRINT("Rect2 size is negative, this is not supported. Use Rect2.abs() to get a Rect2 with a positive size.");
+		}
+#endif
+		Rect2 new_rect;
+
+		new_rect.position = p_rect.position.min(position);
+
+		new_rect.size = (p_rect.position + p_rect.size).max(position + size);
+
+		new_rect.size = new_rect.size - new_rect.position; // Make relative again.
+
+		return new_rect;
+	}
+
+	inline bool has_point(const Point2 &p_point) const {
+#ifdef MATH_CHECKS
+		if (unlikely(size.x < 0 || size.y < 0)) {
+			ERR_PRINT("Rect2 size is negative, this is not supported. Use Rect2.abs() to get a Rect2 with a positive size.");
+		}
+#endif
+		if (p_point.x < position.x) {
+			return false;
+		}
+		if (p_point.y < position.y) {
+			return false;
+		}
+
+		if (p_point.x >= (position.x + size.x)) {
+			return false;
+		}
+		if (p_point.y >= (position.y + size.y)) {
+			return false;
+		}
+
+		return true;
+	}
+
+	bool is_equal_approx(const Rect2 &p_rect) const;
+	bool is_same(const Rect2 &p_rect) const;
+	bool is_finite() const;
+
+	constexpr bool operator==(const Rect2 &p_rect) const { return position == p_rect.position && size == p_rect.size; }
+	constexpr bool operator!=(const Rect2 &p_rect) const { return position != p_rect.position || size != p_rect.size; }
+
+	inline Rect2 grow(real_t p_amount) const {
+		Rect2 g = *this;
+		g.grow_by(p_amount);
+		return g;
+	}
+
+	inline void grow_by(real_t p_amount) {
+		position.x -= p_amount;
+		position.y -= p_amount;
+		size.width += p_amount * 2;
+		size.height += p_amount * 2;
+	}
+
+	inline Rect2 grow_side(Side p_side, real_t p_amount) const {
+		Rect2 g = *this;
+		g = g.grow_individual((SIDE_LEFT == p_side) ? p_amount : 0,
+				(SIDE_TOP == p_side) ? p_amount : 0,
+				(SIDE_RIGHT == p_side) ? p_amount : 0,
+				(SIDE_BOTTOM == p_side) ? p_amount : 0);
+		return g;
+	}
+
+	inline Rect2 grow_side_bind(uint32_t p_side, real_t p_amount) const {
+		return grow_side(Side(p_side), p_amount);
+	}
+
+	inline Rect2 grow_individual(real_t p_left, real_t p_top, real_t p_right, real_t p_bottom) const {
+		Rect2 g = *this;
+		g.position.x -= p_left;
+		g.position.y -= p_top;
+		g.size.width += p_left + p_right;
+		g.size.height += p_top + p_bottom;
+
+		return g;
+	}
+
+	_FORCE_INLINE_ Rect2 expand(const Vector2 &p_vector) const {
+		Rect2 r = *this;
+		r.expand_to(p_vector);
+		return r;
+	}
+
+	inline void expand_to(const Vector2 &p_vector) { // In place function for speed.
+#ifdef MATH_CHECKS
+		if (unlikely(size.x < 0 || size.y < 0)) {
+			ERR_PRINT("Rect2 size is negative, this is not supported. Use Rect2.abs() to get a Rect2 with a positive size.");
+		}
+#endif
+		Vector2 begin = position;
+		Vector2 end = position + size;
+
+		if (p_vector.x < begin.x) {
+			begin.x = p_vector.x;
+		}
+		if (p_vector.y < begin.y) {
+			begin.y = p_vector.y;
+		}
+
+		if (p_vector.x > end.x) {
+			end.x = p_vector.x;
+		}
+		if (p_vector.y > end.y) {
+			end.y = p_vector.y;
+		}
+
+		position = begin;
+		size = end - begin;
+	}
+
+	_FORCE_INLINE_ Rect2 abs() const {
+		return Rect2(position + size.minf(0), size.abs());
+	}
+
+	_FORCE_INLINE_ Rect2 round() const {
+		return Rect2(position.round(), size.round());
+	}
+
+	Vector2 get_support(const Vector2 &p_direction) const {
+		Vector2 support = position;
+		if (p_direction.x > 0.0f) {
+			support.x += size.x;
+		}
+		if (p_direction.y > 0.0f) {
+			support.y += size.y;
+		}
+		return support;
+	}
+
+	_FORCE_INLINE_ bool intersects_filled_polygon(const Vector2 *p_points, int p_point_count) const {
+		Vector2 center = get_center();
+		int side_plus = 0;
+		int side_minus = 0;
+		Vector2 end = position + size;
+
+		int i_f = p_point_count - 1;
+		for (int i = 0; i < p_point_count; i++) {
+			const Vector2 &a = p_points[i_f];
+			const Vector2 &b = p_points[i];
+			i_f = i;
+
+			Vector2 r = (b - a);
+			const real_t l = r.length();
+			if (l == 0.0f) {
+				continue;
+			}
+
+			// Check inside.
+			Vector2 tg = r.orthogonal();
+			const real_t s = tg.dot(center) - tg.dot(a);
+			if (s < 0.0f) {
+				side_plus++;
+			} else {
+				side_minus++;
+			}
+
+			// Check ray box.
+			r /= l;
+			Vector2 ir(1.0f / r.x, 1.0f / r.y);
+
+			// lb is the corner of AABB with minimal coordinates - left bottom, rt is maximal corner
+			// r.org is origin of ray
+			Vector2 t13 = (position - a) * ir;
+			Vector2 t24 = (end - a) * ir;
+
+			const real_t tmin = MAX(MIN(t13.x, t24.x), MIN(t13.y, t24.y));
+			const real_t tmax = MIN(MAX(t13.x, t24.x), MAX(t13.y, t24.y));
+
+			// if tmax < 0, ray (line) is intersecting AABB, but the whole AABB is behind us
+			if (tmax < 0 || tmin > tmax || tmin >= l) {
+				continue;
+			}
+
+			return true;
+		}
+
+		if (side_plus * side_minus == 0) {
+			return true; // All inside.
+		} else {
+			return false;
+		}
+	}
+
+	_FORCE_INLINE_ void set_end(const Vector2 &p_end) {
+		size = p_end - position;
+	}
+
+	_FORCE_INLINE_ Vector2 get_end() const {
+		return position + size;
+	}
+
+	explicit operator String() const;
+	operator Rect2i() const;
+
+	Rect2() = default;
+	constexpr Rect2(real_t p_x, real_t p_y, real_t p_width, real_t p_height) :
+			position(Point2(p_x, p_y)),
+			size(Size2(p_width, p_height)) {
+	}
+	constexpr Rect2(const Point2 &p_pos, const Size2 &p_size) :
+			position(p_pos),
+			size(p_size) {
+	}
+};
+
+template <>
+struct is_zero_constructible<Rect2> : std::true_type {};

core/math/rect2i.cpp

@@ -0,0 +1,42 @@
+/**************************************************************************/
+/*  rect2i.cpp                                                            */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "rect2i.h"
+
+#include "core/math/rect2.h"
+#include "core/string/ustring.h"
+
+Rect2i::operator String() const {
+	return "[P: " + String(position) + ", S: " + String(size) + "]";
+}
+
+Rect2i::operator Rect2() const {
+	return Rect2(position, size);
+}

core/math/rect2i.h

@@ -0,0 +1,241 @@
+/**************************************************************************/
+/*  rect2i.h                                                              */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "core/error/error_macros.h"
+#include "core/math/vector2i.h"
+
+class String;
+struct Rect2;
+
+struct [[nodiscard]] Rect2i {
+	Point2i position;
+	Size2i size;
+
+	const Point2i &get_position() const { return position; }
+	void set_position(const Point2i &p_position) { position = p_position; }
+	const Size2i &get_size() const { return size; }
+	void set_size(const Size2i &p_size) { size = p_size; }
+
+	int get_area() const { return size.width * size.height; }
+
+	_FORCE_INLINE_ Vector2i get_center() const { return position + (size / 2); }
+
+	inline bool intersects(const Rect2i &p_rect) const {
+#ifdef MATH_CHECKS
+		if (unlikely(size.x < 0 || size.y < 0 || p_rect.size.x < 0 || p_rect.size.y < 0)) {
+			ERR_PRINT("Rect2i size is negative, this is not supported. Use Rect2i.abs() to get a Rect2i with a positive size.");
+		}
+#endif
+		if (position.x >= (p_rect.position.x + p_rect.size.width)) {
+			return false;
+		}
+		if ((position.x + size.width) <= p_rect.position.x) {
+			return false;
+		}
+		if (position.y >= (p_rect.position.y + p_rect.size.height)) {
+			return false;
+		}
+		if ((position.y + size.height) <= p_rect.position.y) {
+			return false;
+		}
+
+		return true;
+	}
+
+	inline bool encloses(const Rect2i &p_rect) const {
+#ifdef MATH_CHECKS
+		if (unlikely(size.x < 0 || size.y < 0 || p_rect.size.x < 0 || p_rect.size.y < 0)) {
+			ERR_PRINT("Rect2i size is negative, this is not supported. Use Rect2i.abs() to get a Rect2i with a positive size.");
+		}
+#endif
+		return (p_rect.position.x >= position.x) && (p_rect.position.y >= position.y) &&
+				((p_rect.position.x + p_rect.size.x) <= (position.x + size.x)) &&
+				((p_rect.position.y + p_rect.size.y) <= (position.y + size.y));
+	}
+
+	_FORCE_INLINE_ bool has_area() const {
+		return size.x > 0 && size.y > 0;
+	}
+
+	// Returns the intersection between two Rect2is or an empty Rect2i if there is no intersection.
+	inline Rect2i intersection(const Rect2i &p_rect) const {
+		Rect2i new_rect = p_rect;
+
+		if (!intersects(new_rect)) {
+			return Rect2i();
+		}
+
+		new_rect.position = p_rect.position.max(position);
+
+		Point2i p_rect_end = p_rect.position + p_rect.size;
+		Point2i end = position + size;
+
+		new_rect.size = p_rect_end.min(end) - new_rect.position;
+
+		return new_rect;
+	}
+
+	inline Rect2i merge(const Rect2i &p_rect) const { ///< return a merged rect
+#ifdef MATH_CHECKS
+		if (unlikely(size.x < 0 || size.y < 0 || p_rect.size.x < 0 || p_rect.size.y < 0)) {
+			ERR_PRINT("Rect2i size is negative, this is not supported. Use Rect2i.abs() to get a Rect2i with a positive size.");
+		}
+#endif
+		Rect2i new_rect;
+
+		new_rect.position = p_rect.position.min(position);
+
+		new_rect.size = (p_rect.position + p_rect.size).max(position + size);
+
+		new_rect.size = new_rect.size - new_rect.position; // Make relative again.
+
+		return new_rect;
+	}
+	bool has_point(const Point2i &p_point) const {
+#ifdef MATH_CHECKS
+		if (unlikely(size.x < 0 || size.y < 0)) {
+			ERR_PRINT("Rect2i size is negative, this is not supported. Use Rect2i.abs() to get a Rect2i with a positive size.");
+		}
+#endif
+		if (p_point.x < position.x) {
+			return false;
+		}
+		if (p_point.y < position.y) {
+			return false;
+		}
+
+		if (p_point.x >= (position.x + size.x)) {
+			return false;
+		}
+		if (p_point.y >= (position.y + size.y)) {
+			return false;
+		}
+
+		return true;
+	}
+
+	constexpr bool operator==(const Rect2i &p_rect) const { return position == p_rect.position && size == p_rect.size; }
+	constexpr bool operator!=(const Rect2i &p_rect) const { return position != p_rect.position || size != p_rect.size; }
+
+	Rect2i grow(int p_amount) const {
+		Rect2i g = *this;
+		g.position.x -= p_amount;
+		g.position.y -= p_amount;
+		g.size.width += p_amount * 2;
+		g.size.height += p_amount * 2;
+		return g;
+	}
+
+	inline Rect2i grow_side(Side p_side, int p_amount) const {
+		Rect2i g = *this;
+		g = g.grow_individual((SIDE_LEFT == p_side) ? p_amount : 0,
+				(SIDE_TOP == p_side) ? p_amount : 0,
+				(SIDE_RIGHT == p_side) ? p_amount : 0,
+				(SIDE_BOTTOM == p_side) ? p_amount : 0);
+		return g;
+	}
+
+	inline Rect2i grow_side_bind(uint32_t p_side, int p_amount) const {
+		return grow_side(Side(p_side), p_amount);
+	}
+
+	inline Rect2i grow_individual(int p_left, int p_top, int p_right, int p_bottom) const {
+		Rect2i g = *this;
+		g.position.x -= p_left;
+		g.position.y -= p_top;
+		g.size.width += p_left + p_right;
+		g.size.height += p_top + p_bottom;
+
+		return g;
+	}
+
+	_FORCE_INLINE_ Rect2i expand(const Vector2i &p_vector) const {
+		Rect2i r = *this;
+		r.expand_to(p_vector);
+		return r;
+	}
+
+	inline void expand_to(const Point2i &p_vector) {
+#ifdef MATH_CHECKS
+		if (unlikely(size.x < 0 || size.y < 0)) {
+			ERR_PRINT("Rect2i size is negative, this is not supported. Use Rect2i.abs() to get a Rect2i with a positive size.");
+		}
+#endif
+		Point2i begin = position;
+		Point2i end = position + size;
+
+		if (p_vector.x < begin.x) {
+			begin.x = p_vector.x;
+		}
+		if (p_vector.y < begin.y) {
+			begin.y = p_vector.y;
+		}
+
+		if (p_vector.x > end.x) {
+			end.x = p_vector.x;
+		}
+		if (p_vector.y > end.y) {
+			end.y = p_vector.y;
+		}
+
+		position = begin;
+		size = end - begin;
+	}
+
+	_FORCE_INLINE_ Rect2i abs() const {
+		return Rect2i(position + size.mini(0), size.abs());
+	}
+
+	_FORCE_INLINE_ void set_end(const Vector2i &p_end) {
+		size = p_end - position;
+	}
+
+	_FORCE_INLINE_ Vector2i get_end() const {
+		return position + size;
+	}
+
+	explicit operator String() const;
+	operator Rect2() const;
+
+	Rect2i() = default;
+	constexpr Rect2i(int p_x, int p_y, int p_width, int p_height) :
+			position(Point2i(p_x, p_y)),
+			size(Size2i(p_width, p_height)) {
+	}
+	constexpr Rect2i(const Point2i &p_pos, const Size2i &p_size) :
+			position(p_pos),
+			size(p_size) {
+	}
+};
+
+template <>
+struct is_zero_constructible<Rect2i> : std::true_type {};

core/math/static_raycaster.cpp

@@ -0,0 +1,40 @@
+/**************************************************************************/
+/*  static_raycaster.cpp                                                  */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "static_raycaster.h"
+
+StaticRaycaster *(*StaticRaycaster::create_function)() = nullptr;
+
+Ref<StaticRaycaster> StaticRaycaster::create() {
+	if (create_function) {
+		return Ref<StaticRaycaster>(create_function());
+	}
+	return Ref<StaticRaycaster>();
+}

core/math/static_raycaster.h

@@ -0,0 +1,98 @@
+/**************************************************************************/
+/*  static_raycaster.h                                                    */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "core/object/ref_counted.h"
+
+class StaticRaycaster : public RefCounted {
+	GDCLASS(StaticRaycaster, RefCounted)
+protected:
+	static StaticRaycaster *(*create_function)();
+
+public:
+	// Compatible with embree4 rays.
+	struct alignas(16) Ray {
+		const static unsigned int INVALID_GEOMETRY_ID = ((unsigned int)-1); // from rtcore_common.h
+
+		/*! Default construction does nothing. */
+		_FORCE_INLINE_ Ray() :
+				geomID(INVALID_GEOMETRY_ID) {}
+
+		/*! Constructs a ray from origin, direction, and ray segment. Near
+		 *  has to be smaller than far. */
+		_FORCE_INLINE_ Ray(const Vector3 &p_org,
+				const Vector3 &p_dir,
+				float p_tnear = 0.0f,
+				float p_tfar = Math::INF) :
+				org(p_org),
+				tnear(p_tnear),
+				dir(p_dir),
+				time(0.0f),
+				tfar(p_tfar),
+				mask(-1),
+				u(0.0),
+				v(0.0),
+				primID(INVALID_GEOMETRY_ID),
+				geomID(INVALID_GEOMETRY_ID),
+				instID(INVALID_GEOMETRY_ID) {}
+
+		/*! Tests if we hit something. */
+		_FORCE_INLINE_ explicit operator bool() const { return geomID != INVALID_GEOMETRY_ID; }
+
+	public:
+		Vector3 org; //!< Ray origin + tnear
+		float tnear; //!< Start of ray segment
+		Vector3 dir; //!< Ray direction + tfar
+		float time; //!< Time of this ray for motion blur.
+		float tfar; //!< End of ray segment
+		unsigned int mask; //!< used to mask out objects during traversal
+		unsigned int id; //!< ray ID
+		unsigned int flags; //!< ray flags
+
+		Vector3 normal; //!< Not normalized geometry normal
+		float u; //!< Barycentric u coordinate of hit
+		float v; //!< Barycentric v coordinate of hit
+		unsigned int primID; //!< primitive ID
+		unsigned int geomID; //!< geometry ID
+		unsigned int instID; //!< instance ID
+	};
+
+	virtual bool intersect(Ray &p_ray) = 0;
+	virtual void intersect(Vector<Ray> &r_rays) = 0;
+
+	virtual void add_mesh(const PackedVector3Array &p_vertices, const PackedInt32Array &p_indices, unsigned int p_id) = 0;
+	virtual void commit() = 0;
+
+	virtual void set_mesh_filter(const HashSet<int> &p_mesh_ids) = 0;
+	virtual void clear_mesh_filter() = 0;
+
+	static Ref<StaticRaycaster> create();
+};

core/math/transform_2d.cpp

@@ -0,0 +1,274 @@
+/**************************************************************************/
+/*  transform_2d.cpp                                                      */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "transform_2d.h"
+
+#include "core/string/ustring.h"
+
+void Transform2D::invert() {
+	// FIXME: this function assumes the basis is a rotation matrix, with no scaling.
+	// Transform2D::affine_inverse can handle matrices with scaling, so GDScript should eventually use that.
+	SWAP(columns[0][1], columns[1][0]);
+	columns[2] = basis_xform(-columns[2]);
+}
+
+Transform2D Transform2D::inverse() const {
+	Transform2D inv = *this;
+	inv.invert();
+	return inv;
+}
+
+void Transform2D::affine_invert() {
+	real_t det = determinant();
+#ifdef MATH_CHECKS
+	ERR_FAIL_COND(det == 0);
+#endif
+	real_t idet = 1.0f / det;
+
+	SWAP(columns[0][0], columns[1][1]);
+	columns[0] *= Vector2(idet, -idet);
+	columns[1] *= Vector2(-idet, idet);
+
+	columns[2] = basis_xform(-columns[2]);
+}
+
+Transform2D Transform2D::affine_inverse() const {
+	Transform2D inv = *this;
+	inv.affine_invert();
+	return inv;
+}
+
+void Transform2D::rotate(real_t p_angle) {
+	*this = Transform2D(p_angle, Vector2()) * (*this);
+}
+
+real_t Transform2D::get_skew() const {
+	real_t det = determinant();
+	return Math::acos(columns[0].normalized().dot(SIGN(det) * columns[1].normalized())) - (real_t)Math::PI * 0.5f;
+}
+
+void Transform2D::set_skew(real_t p_angle) {
+	real_t det = determinant();
+	columns[1] = SIGN(det) * columns[0].rotated(((real_t)Math::PI * 0.5f + p_angle)).normalized() * columns[1].length();
+}
+
+real_t Transform2D::get_rotation() const {
+	return Math::atan2(columns[0].y, columns[0].x);
+}
+
+void Transform2D::set_rotation(real_t p_rot) {
+	Size2 scale = get_scale();
+	real_t cr = Math::cos(p_rot);
+	real_t sr = Math::sin(p_rot);
+	columns[0][0] = cr;
+	columns[0][1] = sr;
+	columns[1][0] = -sr;
+	columns[1][1] = cr;
+	set_scale(scale);
+}
+
+Transform2D::Transform2D(real_t p_rot, const Vector2 &p_pos) {
+	real_t cr = Math::cos(p_rot);
+	real_t sr = Math::sin(p_rot);
+	columns[0][0] = cr;
+	columns[0][1] = sr;
+	columns[1][0] = -sr;
+	columns[1][1] = cr;
+	columns[2] = p_pos;
+}
+
+Transform2D::Transform2D(real_t p_rot, const Size2 &p_scale, real_t p_skew, const Vector2 &p_pos) {
+	columns[0][0] = Math::cos(p_rot) * p_scale.x;
+	columns[1][1] = Math::cos(p_rot + p_skew) * p_scale.y;
+	columns[1][0] = -Math::sin(p_rot + p_skew) * p_scale.y;
+	columns[0][1] = Math::sin(p_rot) * p_scale.x;
+	columns[2] = p_pos;
+}
+
+Size2 Transform2D::get_scale() const {
+	real_t det_sign = SIGN(determinant());
+	return Size2(columns[0].length(), det_sign * columns[1].length());
+}
+
+void Transform2D::set_scale(const Size2 &p_scale) {
+	columns[0].normalize();
+	columns[1].normalize();
+	columns[0] *= p_scale.x;
+	columns[1] *= p_scale.y;
+}
+
+void Transform2D::scale(const Size2 &p_scale) {
+	scale_basis(p_scale);
+	columns[2] *= p_scale;
+}
+
+void Transform2D::scale_basis(const Size2 &p_scale) {
+	columns[0][0] *= p_scale.x;
+	columns[0][1] *= p_scale.y;
+	columns[1][0] *= p_scale.x;
+	columns[1][1] *= p_scale.y;
+}
+
+void Transform2D::translate_local(real_t p_tx, real_t p_ty) {
+	translate_local(Vector2(p_tx, p_ty));
+}
+
+void Transform2D::translate_local(const Vector2 &p_translation) {
+	columns[2] += basis_xform(p_translation);
+}
+
+void Transform2D::orthonormalize() {
+	// Gram-Schmidt Process
+
+	Vector2 x = columns[0];
+	Vector2 y = columns[1];
+
+	x.normalize();
+	y = y - x * x.dot(y);
+	y.normalize();
+
+	columns[0] = x;
+	columns[1] = y;
+}
+
+Transform2D Transform2D::orthonormalized() const {
+	Transform2D ortho = *this;
+	ortho.orthonormalize();
+	return ortho;
+}
+
+bool Transform2D::is_conformal() const {
+	// Non-flipped case.
+	if (Math::is_equal_approx(columns[0][0], columns[1][1]) && Math::is_equal_approx(columns[0][1], -columns[1][0])) {
+		return true;
+	}
+	// Flipped case.
+	if (Math::is_equal_approx(columns[0][0], -columns[1][1]) && Math::is_equal_approx(columns[0][1], columns[1][0])) {
+		return true;
+	}
+	return false;
+}
+
+bool Transform2D::is_equal_approx(const Transform2D &p_transform) const {
+	return columns[0].is_equal_approx(p_transform.columns[0]) && columns[1].is_equal_approx(p_transform.columns[1]) && columns[2].is_equal_approx(p_transform.columns[2]);
+}
+
+bool Transform2D::is_same(const Transform2D &p_transform) const {
+	return columns[0].is_same(p_transform.columns[0]) && columns[1].is_same(p_transform.columns[1]) && columns[2].is_same(p_transform.columns[2]);
+}
+
+bool Transform2D::is_finite() const {
+	return columns[0].is_finite() && columns[1].is_finite() && columns[2].is_finite();
+}
+
+Transform2D Transform2D::looking_at(const Vector2 &p_target) const {
+	Transform2D return_trans = Transform2D(get_rotation(), get_origin());
+	Vector2 target_position = affine_inverse().xform(p_target);
+	return_trans.set_rotation(return_trans.get_rotation() + (target_position * get_scale()).angle());
+	return return_trans;
+}
+
+void Transform2D::operator*=(const Transform2D &p_transform) {
+	columns[2] = xform(p_transform.columns[2]);
+
+	real_t x0, x1, y0, y1;
+
+	x0 = tdotx(p_transform.columns[0]);
+	x1 = tdoty(p_transform.columns[0]);
+	y0 = tdotx(p_transform.columns[1]);
+	y1 = tdoty(p_transform.columns[1]);
+
+	columns[0][0] = x0;
+	columns[0][1] = x1;
+	columns[1][0] = y0;
+	columns[1][1] = y1;
+}
+
+Transform2D Transform2D::operator*(const Transform2D &p_transform) const {
+	Transform2D t = *this;
+	t *= p_transform;
+	return t;
+}
+
+Transform2D Transform2D::scaled(const Size2 &p_scale) const {
+	// Equivalent to left multiplication
+	Transform2D copy = *this;
+	copy.scale(p_scale);
+	return copy;
+}
+
+Transform2D Transform2D::scaled_local(const Size2 &p_scale) const {
+	// Equivalent to right multiplication
+	return Transform2D(columns[0] * p_scale.x, columns[1] * p_scale.y, columns[2]);
+}
+
+Transform2D Transform2D::untranslated() const {
+	Transform2D copy = *this;
+	copy.columns[2] = Vector2();
+	return copy;
+}
+
+Transform2D Transform2D::translated(const Vector2 &p_offset) const {
+	// Equivalent to left multiplication
+	return Transform2D(columns[0], columns[1], columns[2] + p_offset);
+}
+
+Transform2D Transform2D::translated_local(const Vector2 &p_offset) const {
+	// Equivalent to right multiplication
+	return Transform2D(columns[0], columns[1], columns[2] + basis_xform(p_offset));
+}
+
+Transform2D Transform2D::rotated(real_t p_angle) const {
+	// Equivalent to left multiplication
+	return Transform2D(p_angle, Vector2()) * (*this);
+}
+
+Transform2D Transform2D::rotated_local(real_t p_angle) const {
+	// Equivalent to right multiplication
+	return (*this) * Transform2D(p_angle, Vector2()); // Could be optimized, because origin transform can be skipped.
+}
+
+real_t Transform2D::determinant() const {
+	return columns[0].x * columns[1].y - columns[0].y * columns[1].x;
+}
+
+Transform2D Transform2D::interpolate_with(const Transform2D &p_transform, real_t p_weight) const {
+	return Transform2D(
+			Math::lerp_angle(get_rotation(), p_transform.get_rotation(), p_weight),
+			get_scale().lerp(p_transform.get_scale(), p_weight),
+			Math::lerp_angle(get_skew(), p_transform.get_skew(), p_weight),
+			get_origin().lerp(p_transform.get_origin(), p_weight));
+}
+
+Transform2D::operator String() const {
+	return "[X: " + columns[0].operator String() +
+			", Y: " + columns[1].operator String() +
+			", O: " + columns[2].operator String() + "]";
+}

core/math/transform_2d.h

@@ -0,0 +1,291 @@
+/**************************************************************************/
+/*  transform_2d.h                                                        */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "core/math/math_funcs.h"
+#include "core/math/rect2.h"
+#include "core/math/vector2.h"
+#include "core/templates/vector.h"
+
+class String;
+
+struct [[nodiscard]] Transform2D {
+	// WARNING: The basis of Transform2D is stored differently from Basis.
+	// In terms of columns array, the basis matrix looks like "on paper":
+	// M = (columns[0][0] columns[1][0])
+	//     (columns[0][1] columns[1][1])
+	// This is such that the columns, which can be interpreted as basis vectors
+	// of the coordinate system "painted" on the object, can be accessed as columns[i].
+	// NOTE: This is the opposite of the indices in mathematical texts,
+	// meaning: $M_{12}$ in a math book corresponds to columns[1][0] here.
+	// This requires additional care when working with explicit indices.
+	// See https://en.wikipedia.org/wiki/Row-_and_column-major_order for further reading.
+
+	// WARNING: Be aware that unlike 3D code, 2D code uses a left-handed coordinate system:
+	// Y-axis points down, and angle is measure from +X to +Y in a clockwise-fashion.
+
+	Vector2 columns[3] = {
+		{ 1, 0 },
+		{ 0, 1 },
+		{ 0, 0 },
+	};
+
+	_FORCE_INLINE_ real_t tdotx(const Vector2 &p_v) const { return columns[0][0] * p_v.x + columns[1][0] * p_v.y; }
+	_FORCE_INLINE_ real_t tdoty(const Vector2 &p_v) const { return columns[0][1] * p_v.x + columns[1][1] * p_v.y; }
+
+	constexpr const Vector2 &operator[](int p_idx) const { return columns[p_idx]; }
+	constexpr Vector2 &operator[](int p_idx) { return columns[p_idx]; }
+
+	void invert();
+	Transform2D inverse() const;
+
+	void affine_invert();
+	Transform2D affine_inverse() const;
+
+	void set_rotation(real_t p_rot);
+	real_t get_rotation() const;
+	real_t get_skew() const;
+	void set_skew(real_t p_angle);
+	_FORCE_INLINE_ void set_rotation_and_scale(real_t p_rot, const Size2 &p_scale);
+	_FORCE_INLINE_ void set_rotation_scale_and_skew(real_t p_rot, const Size2 &p_scale, real_t p_skew);
+	void rotate(real_t p_angle);
+
+	void scale(const Size2 &p_scale);
+	void scale_basis(const Size2 &p_scale);
+	void translate_local(real_t p_tx, real_t p_ty);
+	void translate_local(const Vector2 &p_translation);
+
+	real_t determinant() const;
+
+	Size2 get_scale() const;
+	void set_scale(const Size2 &p_scale);
+
+	_FORCE_INLINE_ const Vector2 &get_origin() const { return columns[2]; }
+	_FORCE_INLINE_ void set_origin(const Vector2 &p_origin) { columns[2] = p_origin; }
+
+	Transform2D scaled(const Size2 &p_scale) const;
+	Transform2D scaled_local(const Size2 &p_scale) const;
+	Transform2D translated(const Vector2 &p_offset) const;
+	Transform2D translated_local(const Vector2 &p_offset) const;
+	Transform2D rotated(real_t p_angle) const;
+	Transform2D rotated_local(real_t p_angle) const;
+
+	Transform2D untranslated() const;
+
+	void orthonormalize();
+	Transform2D orthonormalized() const;
+	bool is_conformal() const;
+	bool is_equal_approx(const Transform2D &p_transform) const;
+	bool is_same(const Transform2D &p_transform) const;
+	bool is_finite() const;
+
+	Transform2D looking_at(const Vector2 &p_target) const;
+
+	constexpr bool operator==(const Transform2D &p_transform) const;
+	constexpr bool operator!=(const Transform2D &p_transform) const;
+
+	void operator*=(const Transform2D &p_transform);
+	Transform2D operator*(const Transform2D &p_transform) const;
+	constexpr void operator*=(real_t p_val);
+	constexpr Transform2D operator*(real_t p_val) const;
+	constexpr void operator/=(real_t p_val);
+	constexpr Transform2D operator/(real_t p_val) const;
+
+	Transform2D interpolate_with(const Transform2D &p_transform, real_t p_c) const;
+
+	_FORCE_INLINE_ Vector2 basis_xform(const Vector2 &p_vec) const;
+	_FORCE_INLINE_ Vector2 basis_xform_inv(const Vector2 &p_vec) const;
+	_FORCE_INLINE_ Vector2 xform(const Vector2 &p_vec) const;
+	_FORCE_INLINE_ Vector2 xform_inv(const Vector2 &p_vec) const;
+	_FORCE_INLINE_ Rect2 xform(const Rect2 &p_rect) const;
+	_FORCE_INLINE_ Rect2 xform_inv(const Rect2 &p_rect) const;
+	_FORCE_INLINE_ Vector<Vector2> xform(const Vector<Vector2> &p_array) const;
+	_FORCE_INLINE_ Vector<Vector2> xform_inv(const Vector<Vector2> &p_array) const;
+
+	explicit operator String() const;
+
+	constexpr Transform2D(real_t p_xx, real_t p_xy, real_t p_yx, real_t p_yy, real_t p_ox, real_t p_oy) :
+			columns{
+				{ p_xx, p_xy },
+				{ p_yx, p_yy },
+				{ p_ox, p_oy },
+			} {}
+
+	constexpr Transform2D(const Vector2 &p_x, const Vector2 &p_y, const Vector2 &p_origin) :
+			columns{ p_x, p_y, p_origin } {}
+
+	Transform2D(real_t p_rot, const Vector2 &p_pos);
+
+	Transform2D(real_t p_rot, const Size2 &p_scale, real_t p_skew, const Vector2 &p_pos);
+
+	Transform2D() = default;
+};
+
+constexpr bool Transform2D::operator==(const Transform2D &p_transform) const {
+	for (int i = 0; i < 3; i++) {
+		if (columns[i] != p_transform.columns[i]) {
+			return false;
+		}
+	}
+
+	return true;
+}
+
+constexpr bool Transform2D::operator!=(const Transform2D &p_transform) const {
+	for (int i = 0; i < 3; i++) {
+		if (columns[i] != p_transform.columns[i]) {
+			return true;
+		}
+	}
+
+	return false;
+}
+
+constexpr void Transform2D::operator*=(real_t p_val) {
+	columns[0] *= p_val;
+	columns[1] *= p_val;
+	columns[2] *= p_val;
+}
+
+constexpr Transform2D Transform2D::operator*(real_t p_val) const {
+	Transform2D ret(*this);
+	ret *= p_val;
+	return ret;
+}
+
+constexpr void Transform2D::operator/=(real_t p_val) {
+	columns[0] /= p_val;
+	columns[1] /= p_val;
+	columns[2] /= p_val;
+}
+
+constexpr Transform2D Transform2D::operator/(real_t p_val) const {
+	Transform2D ret(*this);
+	ret /= p_val;
+	return ret;
+}
+
+Vector2 Transform2D::basis_xform(const Vector2 &p_vec) const {
+	return Vector2(
+			tdotx(p_vec),
+			tdoty(p_vec));
+}
+
+Vector2 Transform2D::basis_xform_inv(const Vector2 &p_vec) const {
+	return Vector2(
+			columns[0].dot(p_vec),
+			columns[1].dot(p_vec));
+}
+
+Vector2 Transform2D::xform(const Vector2 &p_vec) const {
+	return Vector2(
+				   tdotx(p_vec),
+				   tdoty(p_vec)) +
+			columns[2];
+}
+
+Vector2 Transform2D::xform_inv(const Vector2 &p_vec) const {
+	Vector2 v = p_vec - columns[2];
+
+	return Vector2(
+			columns[0].dot(v),
+			columns[1].dot(v));
+}
+
+Rect2 Transform2D::xform(const Rect2 &p_rect) const {
+	Vector2 x = columns[0] * p_rect.size.x;
+	Vector2 y = columns[1] * p_rect.size.y;
+	Vector2 pos = xform(p_rect.position);
+
+	Rect2 new_rect;
+	new_rect.position = pos;
+	new_rect.expand_to(pos + x);
+	new_rect.expand_to(pos + y);
+	new_rect.expand_to(pos + x + y);
+	return new_rect;
+}
+
+void Transform2D::set_rotation_and_scale(real_t p_rot, const Size2 &p_scale) {
+	columns[0][0] = Math::cos(p_rot) * p_scale.x;
+	columns[1][1] = Math::cos(p_rot) * p_scale.y;
+	columns[1][0] = -Math::sin(p_rot) * p_scale.y;
+	columns[0][1] = Math::sin(p_rot) * p_scale.x;
+}
+
+void Transform2D::set_rotation_scale_and_skew(real_t p_rot, const Size2 &p_scale, real_t p_skew) {
+	columns[0][0] = Math::cos(p_rot) * p_scale.x;
+	columns[1][1] = Math::cos(p_rot + p_skew) * p_scale.y;
+	columns[1][0] = -Math::sin(p_rot + p_skew) * p_scale.y;
+	columns[0][1] = Math::sin(p_rot) * p_scale.x;
+}
+
+Rect2 Transform2D::xform_inv(const Rect2 &p_rect) const {
+	Vector2 ends[4] = {
+		xform_inv(p_rect.position),
+		xform_inv(Vector2(p_rect.position.x, p_rect.position.y + p_rect.size.y)),
+		xform_inv(Vector2(p_rect.position.x + p_rect.size.x, p_rect.position.y + p_rect.size.y)),
+		xform_inv(Vector2(p_rect.position.x + p_rect.size.x, p_rect.position.y))
+	};
+
+	Rect2 new_rect;
+	new_rect.position = ends[0];
+	new_rect.expand_to(ends[1]);
+	new_rect.expand_to(ends[2]);
+	new_rect.expand_to(ends[3]);
+
+	return new_rect;
+}
+
+Vector<Vector2> Transform2D::xform(const Vector<Vector2> &p_array) const {
+	Vector<Vector2> array;
+	array.resize(p_array.size());
+
+	const Vector2 *r = p_array.ptr();
+	Vector2 *w = array.ptrw();
+
+	for (int i = 0; i < p_array.size(); ++i) {
+		w[i] = xform(r[i]);
+	}
+	return array;
+}
+
+Vector<Vector2> Transform2D::xform_inv(const Vector<Vector2> &p_array) const {
+	Vector<Vector2> array;
+	array.resize(p_array.size());
+
+	const Vector2 *r = p_array.ptr();
+	Vector2 *w = array.ptrw();
+
+	for (int i = 0; i < p_array.size(); ++i) {
+		w[i] = xform_inv(r[i]);
+	}
+	return array;
+}

core/math/transform_3d.cpp

@@ -0,0 +1,200 @@
+/**************************************************************************/
+/*  transform_3d.cpp                                                      */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "transform_3d.h"
+
+#include "core/string/ustring.h"
+
+void Transform3D::affine_invert() {
+	basis.invert();
+	origin = basis.xform(-origin);
+}
+
+Transform3D Transform3D::affine_inverse() const {
+	Transform3D ret = *this;
+	ret.affine_invert();
+	return ret;
+}
+
+void Transform3D::invert() {
+	basis.transpose();
+	origin = basis.xform(-origin);
+}
+
+Transform3D Transform3D::inverse() const {
+	// FIXME: this function assumes the basis is a rotation matrix, with no scaling.
+	// Transform3D::affine_inverse can handle matrices with scaling, so GDScript should eventually use that.
+	Transform3D ret = *this;
+	ret.invert();
+	return ret;
+}
+
+void Transform3D::rotate(const Vector3 &p_axis, real_t p_angle) {
+	*this = rotated(p_axis, p_angle);
+}
+
+Transform3D Transform3D::rotated(const Vector3 &p_axis, real_t p_angle) const {
+	// Equivalent to left multiplication
+	Basis p_basis(p_axis, p_angle);
+	return Transform3D(p_basis * basis, p_basis.xform(origin));
+}
+
+Transform3D Transform3D::rotated_local(const Vector3 &p_axis, real_t p_angle) const {
+	// Equivalent to right multiplication
+	Basis p_basis(p_axis, p_angle);
+	return Transform3D(basis * p_basis, origin);
+}
+
+void Transform3D::rotate_basis(const Vector3 &p_axis, real_t p_angle) {
+	basis.rotate(p_axis, p_angle);
+}
+
+Transform3D Transform3D::looking_at(const Vector3 &p_target, const Vector3 &p_up, bool p_use_model_front) const {
+#ifdef MATH_CHECKS
+	ERR_FAIL_COND_V_MSG(origin.is_equal_approx(p_target), Transform3D(), "The transform's origin and target can't be equal.");
+#endif
+	Transform3D t = *this;
+	t.basis = Basis::looking_at(p_target - origin, p_up, p_use_model_front);
+	return t;
+}
+
+void Transform3D::set_look_at(const Vector3 &p_eye, const Vector3 &p_target, const Vector3 &p_up, bool p_use_model_front) {
+#ifdef MATH_CHECKS
+	ERR_FAIL_COND_MSG(p_eye.is_equal_approx(p_target), "The eye and target vectors can't be equal.");
+#endif
+	basis = Basis::looking_at(p_target - p_eye, p_up, p_use_model_front);
+	origin = p_eye;
+}
+
+Transform3D Transform3D::interpolate_with(const Transform3D &p_transform, real_t p_c) const {
+	Transform3D interp;
+
+	Vector3 src_scale = basis.get_scale();
+	Quaternion src_rot = basis.get_rotation_quaternion();
+	Vector3 src_loc = origin;
+
+	Vector3 dst_scale = p_transform.basis.get_scale();
+	Quaternion dst_rot = p_transform.basis.get_rotation_quaternion();
+	Vector3 dst_loc = p_transform.origin;
+
+	interp.basis.set_quaternion_scale(src_rot.slerp(dst_rot, p_c).normalized(), src_scale.lerp(dst_scale, p_c));
+	interp.origin = src_loc.lerp(dst_loc, p_c);
+
+	return interp;
+}
+
+void Transform3D::scale(const Vector3 &p_scale) {
+	basis.scale(p_scale);
+	origin *= p_scale;
+}
+
+Transform3D Transform3D::scaled(const Vector3 &p_scale) const {
+	// Equivalent to left multiplication
+	return Transform3D(basis.scaled(p_scale), origin * p_scale);
+}
+
+Transform3D Transform3D::scaled_local(const Vector3 &p_scale) const {
+	// Equivalent to right multiplication
+	return Transform3D(basis.scaled_local(p_scale), origin);
+}
+
+void Transform3D::scale_basis(const Vector3 &p_scale) {
+	basis.scale(p_scale);
+}
+
+void Transform3D::translate_local(real_t p_tx, real_t p_ty, real_t p_tz) {
+	translate_local(Vector3(p_tx, p_ty, p_tz));
+}
+
+void Transform3D::translate_local(const Vector3 &p_translation) {
+	for (int i = 0; i < 3; i++) {
+		origin[i] += basis[i].dot(p_translation);
+	}
+}
+
+Transform3D Transform3D::translated(const Vector3 &p_translation) const {
+	// Equivalent to left multiplication
+	return Transform3D(basis, origin + p_translation);
+}
+
+Transform3D Transform3D::translated_local(const Vector3 &p_translation) const {
+	// Equivalent to right multiplication
+	return Transform3D(basis, origin + basis.xform(p_translation));
+}
+
+void Transform3D::orthonormalize() {
+	basis.orthonormalize();
+}
+
+Transform3D Transform3D::orthonormalized() const {
+	Transform3D _copy = *this;
+	_copy.orthonormalize();
+	return _copy;
+}
+
+void Transform3D::orthogonalize() {
+	basis.orthogonalize();
+}
+
+Transform3D Transform3D::orthogonalized() const {
+	Transform3D _copy = *this;
+	_copy.orthogonalize();
+	return _copy;
+}
+
+bool Transform3D::is_equal_approx(const Transform3D &p_transform) const {
+	return basis.is_equal_approx(p_transform.basis) && origin.is_equal_approx(p_transform.origin);
+}
+
+bool Transform3D::is_same(const Transform3D &p_transform) const {
+	return basis.is_same(p_transform.basis) && origin.is_same(p_transform.origin);
+}
+
+bool Transform3D::is_finite() const {
+	return basis.is_finite() && origin.is_finite();
+}
+
+void Transform3D::operator*=(const Transform3D &p_transform) {
+	origin = xform(p_transform.origin);
+	basis *= p_transform.basis;
+}
+
+Transform3D Transform3D::operator*(const Transform3D &p_transform) const {
+	Transform3D t = *this;
+	t *= p_transform;
+	return t;
+}
+
+Transform3D::operator String() const {
+	return "[X: " + basis.get_column(0).operator String() +
+			", Y: " + basis.get_column(1).operator String() +
+			", Z: " + basis.get_column(2).operator String() +
+			", O: " + origin.operator String() + "]";
+}

core/math/transform_3d.h

@@ -0,0 +1,307 @@
+/**************************************************************************/
+/*  transform_3d.h                                                        */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "core/math/aabb.h"
+#include "core/math/basis.h"
+#include "core/math/plane.h"
+#include "core/templates/vector.h"
+
+struct [[nodiscard]] Transform3D {
+	Basis basis;
+	Vector3 origin;
+
+	void invert();
+	Transform3D inverse() const;
+
+	void affine_invert();
+	Transform3D affine_inverse() const;
+
+	Transform3D rotated(const Vector3 &p_axis, real_t p_angle) const;
+	Transform3D rotated_local(const Vector3 &p_axis, real_t p_angle) const;
+
+	void rotate(const Vector3 &p_axis, real_t p_angle);
+	void rotate_basis(const Vector3 &p_axis, real_t p_angle);
+
+	void set_look_at(const Vector3 &p_eye, const Vector3 &p_target, const Vector3 &p_up = Vector3(0, 1, 0), bool p_use_model_front = false);
+	Transform3D looking_at(const Vector3 &p_target, const Vector3 &p_up = Vector3(0, 1, 0), bool p_use_model_front = false) const;
+
+	void scale(const Vector3 &p_scale);
+	Transform3D scaled(const Vector3 &p_scale) const;
+	Transform3D scaled_local(const Vector3 &p_scale) const;
+	void scale_basis(const Vector3 &p_scale);
+	void translate_local(real_t p_tx, real_t p_ty, real_t p_tz);
+	void translate_local(const Vector3 &p_translation);
+	Transform3D translated(const Vector3 &p_translation) const;
+	Transform3D translated_local(const Vector3 &p_translation) const;
+
+	const Basis &get_basis() const { return basis; }
+	void set_basis(const Basis &p_basis) { basis = p_basis; }
+
+	const Vector3 &get_origin() const { return origin; }
+	void set_origin(const Vector3 &p_origin) { origin = p_origin; }
+
+	void orthonormalize();
+	Transform3D orthonormalized() const;
+	void orthogonalize();
+	Transform3D orthogonalized() const;
+	bool is_equal_approx(const Transform3D &p_transform) const;
+	bool is_same(const Transform3D &p_transform) const;
+	bool is_finite() const;
+
+	constexpr bool operator==(const Transform3D &p_transform) const;
+	constexpr bool operator!=(const Transform3D &p_transform) const;
+
+	_FORCE_INLINE_ Vector3 xform(const Vector3 &p_vector) const;
+	_FORCE_INLINE_ AABB xform(const AABB &p_aabb) const;
+	_FORCE_INLINE_ Vector<Vector3> xform(const Vector<Vector3> &p_array) const;
+
+	// NOTE: These are UNSAFE with non-uniform scaling, and will produce incorrect results.
+	// They use the transpose.
+	// For safe inverse transforms, xform by the affine_inverse.
+	_FORCE_INLINE_ Vector3 xform_inv(const Vector3 &p_vector) const;
+	_FORCE_INLINE_ AABB xform_inv(const AABB &p_aabb) const;
+	_FORCE_INLINE_ Vector<Vector3> xform_inv(const Vector<Vector3> &p_array) const;
+
+	// Safe with non-uniform scaling (uses affine_inverse).
+	_FORCE_INLINE_ Plane xform(const Plane &p_plane) const;
+	_FORCE_INLINE_ Plane xform_inv(const Plane &p_plane) const;
+
+	// These fast versions use precomputed affine inverse, and should be used in bottleneck areas where
+	// multiple planes are to be transformed.
+	_FORCE_INLINE_ Plane xform_fast(const Plane &p_plane, const Basis &p_basis_inverse_transpose) const;
+	static _FORCE_INLINE_ Plane xform_inv_fast(const Plane &p_plane, const Transform3D &p_inverse, const Basis &p_basis_transpose);
+
+	void operator*=(const Transform3D &p_transform);
+	Transform3D operator*(const Transform3D &p_transform) const;
+	constexpr void operator*=(real_t p_val);
+	constexpr Transform3D operator*(real_t p_val) const;
+	constexpr void operator/=(real_t p_val);
+	constexpr Transform3D operator/(real_t p_val) const;
+
+	Transform3D interpolate_with(const Transform3D &p_transform, real_t p_c) const;
+
+	_FORCE_INLINE_ Transform3D inverse_xform(const Transform3D &t) const {
+		Vector3 v = t.origin - origin;
+		return Transform3D(basis.transpose_xform(t.basis),
+				basis.xform(v));
+	}
+
+	void set(real_t p_xx, real_t p_xy, real_t p_xz, real_t p_yx, real_t p_yy, real_t p_yz, real_t p_zx, real_t p_zy, real_t p_zz, real_t p_tx, real_t p_ty, real_t p_tz) {
+		basis.set(p_xx, p_xy, p_xz, p_yx, p_yy, p_yz, p_zx, p_zy, p_zz);
+		origin.x = p_tx;
+		origin.y = p_ty;
+		origin.z = p_tz;
+	}
+
+	explicit operator String() const;
+
+	Transform3D() = default;
+	constexpr Transform3D(const Basis &p_basis, const Vector3 &p_origin = Vector3()) :
+			basis(p_basis),
+			origin(p_origin) {}
+	constexpr Transform3D(const Vector3 &p_x, const Vector3 &p_y, const Vector3 &p_z, const Vector3 &p_origin) :
+			basis(p_x, p_y, p_z),
+			origin(p_origin) {}
+	constexpr Transform3D(real_t p_xx, real_t p_xy, real_t p_xz, real_t p_yx, real_t p_yy, real_t p_yz, real_t p_zx, real_t p_zy, real_t p_zz, real_t p_ox, real_t p_oy, real_t p_oz) :
+			basis(p_xx, p_xy, p_xz, p_yx, p_yy, p_yz, p_zx, p_zy, p_zz),
+			origin(p_ox, p_oy, p_oz) {}
+};
+
+constexpr bool Transform3D::operator==(const Transform3D &p_transform) const {
+	return (basis == p_transform.basis && origin == p_transform.origin);
+}
+
+constexpr bool Transform3D::operator!=(const Transform3D &p_transform) const {
+	return (basis != p_transform.basis || origin != p_transform.origin);
+}
+
+constexpr void Transform3D::operator*=(real_t p_val) {
+	origin *= p_val;
+	basis *= p_val;
+}
+
+constexpr Transform3D Transform3D::operator*(real_t p_val) const {
+	Transform3D ret(*this);
+	ret *= p_val;
+	return ret;
+}
+
+constexpr void Transform3D::operator/=(real_t p_val) {
+	basis /= p_val;
+	origin /= p_val;
+}
+
+constexpr Transform3D Transform3D::operator/(real_t p_val) const {
+	Transform3D ret(*this);
+	ret /= p_val;
+	return ret;
+}
+
+_FORCE_INLINE_ Vector3 Transform3D::xform(const Vector3 &p_vector) const {
+	return Vector3(
+			basis[0].dot(p_vector) + origin.x,
+			basis[1].dot(p_vector) + origin.y,
+			basis[2].dot(p_vector) + origin.z);
+}
+
+_FORCE_INLINE_ Vector3 Transform3D::xform_inv(const Vector3 &p_vector) const {
+	Vector3 v = p_vector - origin;
+
+	return Vector3(
+			(basis.rows[0][0] * v.x) + (basis.rows[1][0] * v.y) + (basis.rows[2][0] * v.z),
+			(basis.rows[0][1] * v.x) + (basis.rows[1][1] * v.y) + (basis.rows[2][1] * v.z),
+			(basis.rows[0][2] * v.x) + (basis.rows[1][2] * v.y) + (basis.rows[2][2] * v.z));
+}
+
+// Neither the plane regular xform or xform_inv are particularly efficient,
+// as they do a basis inverse. For xforming a large number
+// of planes it is better to pre-calculate the inverse transpose basis once
+// and reuse it for each plane, by using the 'fast' version of the functions.
+_FORCE_INLINE_ Plane Transform3D::xform(const Plane &p_plane) const {
+	Basis b = basis.inverse();
+	b.transpose();
+	return xform_fast(p_plane, b);
+}
+
+_FORCE_INLINE_ Plane Transform3D::xform_inv(const Plane &p_plane) const {
+	Transform3D inv = affine_inverse();
+	Basis basis_transpose = basis.transposed();
+	return xform_inv_fast(p_plane, inv, basis_transpose);
+}
+
+_FORCE_INLINE_ AABB Transform3D::xform(const AABB &p_aabb) const {
+	/* https://dev.theomader.com/transform-bounding-boxes/ */
+	Vector3 min = p_aabb.position;
+	Vector3 max = p_aabb.position + p_aabb.size;
+	Vector3 tmin, tmax;
+	for (int i = 0; i < 3; i++) {
+		tmin[i] = tmax[i] = origin[i];
+		for (int j = 0; j < 3; j++) {
+			real_t e = basis[i][j] * min[j];
+			real_t f = basis[i][j] * max[j];
+			if (e < f) {
+				tmin[i] += e;
+				tmax[i] += f;
+			} else {
+				tmin[i] += f;
+				tmax[i] += e;
+			}
+		}
+	}
+	AABB r_aabb;
+	r_aabb.position = tmin;
+	r_aabb.size = tmax - tmin;
+	return r_aabb;
+}
+
+_FORCE_INLINE_ AABB Transform3D::xform_inv(const AABB &p_aabb) const {
+	/* define vertices */
+	Vector3 vertices[8] = {
+		Vector3(p_aabb.position.x + p_aabb.size.x, p_aabb.position.y + p_aabb.size.y, p_aabb.position.z + p_aabb.size.z),
+		Vector3(p_aabb.position.x + p_aabb.size.x, p_aabb.position.y + p_aabb.size.y, p_aabb.position.z),
+		Vector3(p_aabb.position.x + p_aabb.size.x, p_aabb.position.y, p_aabb.position.z + p_aabb.size.z),
+		Vector3(p_aabb.position.x + p_aabb.size.x, p_aabb.position.y, p_aabb.position.z),
+		Vector3(p_aabb.position.x, p_aabb.position.y + p_aabb.size.y, p_aabb.position.z + p_aabb.size.z),
+		Vector3(p_aabb.position.x, p_aabb.position.y + p_aabb.size.y, p_aabb.position.z),
+		Vector3(p_aabb.position.x, p_aabb.position.y, p_aabb.position.z + p_aabb.size.z),
+		Vector3(p_aabb.position.x, p_aabb.position.y, p_aabb.position.z)
+	};
+
+	AABB ret;
+
+	ret.position = xform_inv(vertices[0]);
+
+	for (int i = 1; i < 8; i++) {
+		ret.expand_to(xform_inv(vertices[i]));
+	}
+
+	return ret;
+}
+
+Vector<Vector3> Transform3D::xform(const Vector<Vector3> &p_array) const {
+	Vector<Vector3> array;
+	array.resize(p_array.size());
+
+	const Vector3 *r = p_array.ptr();
+	Vector3 *w = array.ptrw();
+
+	for (int i = 0; i < p_array.size(); ++i) {
+		w[i] = xform(r[i]);
+	}
+	return array;
+}
+
+Vector<Vector3> Transform3D::xform_inv(const Vector<Vector3> &p_array) const {
+	Vector<Vector3> array;
+	array.resize(p_array.size());
+
+	const Vector3 *r = p_array.ptr();
+	Vector3 *w = array.ptrw();
+
+	for (int i = 0; i < p_array.size(); ++i) {
+		w[i] = xform_inv(r[i]);
+	}
+	return array;
+}
+
+_FORCE_INLINE_ Plane Transform3D::xform_fast(const Plane &p_plane, const Basis &p_basis_inverse_transpose) const {
+	// Transform a single point on the plane.
+	Vector3 point = p_plane.normal * p_plane.d;
+	point = xform(point);
+
+	// Use inverse transpose for correct normals with non-uniform scaling.
+	Vector3 normal = p_basis_inverse_transpose.xform(p_plane.normal);
+	normal.normalize();
+
+	real_t d = normal.dot(point);
+	return Plane(normal, d);
+}
+
+_FORCE_INLINE_ Plane Transform3D::xform_inv_fast(const Plane &p_plane, const Transform3D &p_inverse, const Basis &p_basis_transpose) {
+	// Transform a single point on the plane.
+	Vector3 point = p_plane.normal * p_plane.d;
+	point = p_inverse.xform(point);
+
+	// Note that instead of precalculating the transpose, an alternative
+	// would be to use the transpose for the basis transform.
+	// However that would be less SIMD friendly (requiring a swizzle).
+	// So the cost is one extra precalced value in the calling code.
+	// This is probably worth it, as this could be used in bottleneck areas. And
+	// where it is not a bottleneck, the non-fast method is fine.
+
+	// Use transpose for correct normals with non-uniform scaling.
+	Vector3 normal = p_basis_transpose.xform(p_plane.normal);
+	normal.normalize();
+
+	real_t d = normal.dot(point);
+	return Plane(normal, d);
+}

core/math/transform_interpolator.cpp

@@ -0,0 +1,384 @@
+/**************************************************************************/
+/*  transform_interpolator.cpp                                            */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "transform_interpolator.h"
+
+#include "core/math/transform_2d.h"
+#include "core/math/transform_3d.h"
+
+void TransformInterpolator::interpolate_transform_2d(const Transform2D &p_prev, const Transform2D &p_curr, Transform2D &r_result, real_t p_fraction) {
+	// Special case for physics interpolation, if flipping, don't interpolate basis.
+	// If the determinant polarity changes, the handedness of the coordinate system changes.
+	if (_sign(p_prev.determinant()) != _sign(p_curr.determinant())) {
+		r_result.columns[0] = p_curr.columns[0];
+		r_result.columns[1] = p_curr.columns[1];
+		r_result.set_origin(p_prev.get_origin().lerp(p_curr.get_origin(), p_fraction));
+		return;
+	}
+
+	r_result = p_prev.interpolate_with(p_curr, p_fraction);
+}
+
+void TransformInterpolator::interpolate_transform_3d(const Transform3D &p_prev, const Transform3D &p_curr, Transform3D &r_result, real_t p_fraction) {
+	r_result.origin = p_prev.origin + ((p_curr.origin - p_prev.origin) * p_fraction);
+	interpolate_basis(p_prev.basis, p_curr.basis, r_result.basis, p_fraction);
+}
+
+void TransformInterpolator::interpolate_basis(const Basis &p_prev, const Basis &p_curr, Basis &r_result, real_t p_fraction) {
+	Method method = find_method(p_prev, p_curr);
+	interpolate_basis_via_method(p_prev, p_curr, r_result, p_fraction, method);
+}
+
+void TransformInterpolator::interpolate_transform_3d_via_method(const Transform3D &p_prev, const Transform3D &p_curr, Transform3D &r_result, real_t p_fraction, Method p_method) {
+	r_result.origin = p_prev.origin + ((p_curr.origin - p_prev.origin) * p_fraction);
+	interpolate_basis_via_method(p_prev.basis, p_curr.basis, r_result.basis, p_fraction, p_method);
+}
+
+void TransformInterpolator::interpolate_basis_via_method(const Basis &p_prev, const Basis &p_curr, Basis &r_result, real_t p_fraction, Method p_method) {
+	switch (p_method) {
+		default: {
+			interpolate_basis_linear(p_prev, p_curr, r_result, p_fraction);
+		} break;
+		case INTERP_SLERP: {
+			r_result = _basis_slerp_unchecked(p_prev, p_curr, p_fraction);
+		} break;
+		case INTERP_SCALED_SLERP: {
+			interpolate_basis_scaled_slerp(p_prev, p_curr, r_result, p_fraction);
+		} break;
+	}
+}
+
+Quaternion TransformInterpolator::_basis_to_quat_unchecked(const Basis &p_basis) {
+	Basis m = p_basis;
+	real_t trace = m.rows[0][0] + m.rows[1][1] + m.rows[2][2];
+	real_t temp[4];
+
+	if (trace > 0.0) {
+		real_t s = Math::sqrt(trace + 1.0f);
+		temp[3] = (s * 0.5f);
+		s = 0.5f / s;
+
+		temp[0] = ((m.rows[2][1] - m.rows[1][2]) * s);
+		temp[1] = ((m.rows[0][2] - m.rows[2][0]) * s);
+		temp[2] = ((m.rows[1][0] - m.rows[0][1]) * s);
+	} else {
+		int i = m.rows[0][0] < m.rows[1][1]
+				? (m.rows[1][1] < m.rows[2][2] ? 2 : 1)
+				: (m.rows[0][0] < m.rows[2][2] ? 2 : 0);
+		int j = (i + 1) % 3;
+		int k = (i + 2) % 3;
+
+		real_t s = Math::sqrt(m.rows[i][i] - m.rows[j][j] - m.rows[k][k] + 1.0f);
+		temp[i] = s * 0.5f;
+		s = 0.5f / s;
+
+		temp[3] = (m.rows[k][j] - m.rows[j][k]) * s;
+		temp[j] = (m.rows[j][i] + m.rows[i][j]) * s;
+		temp[k] = (m.rows[k][i] + m.rows[i][k]) * s;
+	}
+
+	return Quaternion(temp[0], temp[1], temp[2], temp[3]);
+}
+
+Quaternion TransformInterpolator::_quat_slerp_unchecked(const Quaternion &p_from, const Quaternion &p_to, real_t p_fraction) {
+	Quaternion to1;
+	real_t omega, cosom, sinom, scale0, scale1;
+
+	// Calculate cosine.
+	cosom = p_from.dot(p_to);
+
+	// Adjust signs (if necessary)
+	if (cosom < 0.0f) {
+		cosom = -cosom;
+		to1.x = -p_to.x;
+		to1.y = -p_to.y;
+		to1.z = -p_to.z;
+		to1.w = -p_to.w;
+	} else {
+		to1.x = p_to.x;
+		to1.y = p_to.y;
+		to1.z = p_to.z;
+		to1.w = p_to.w;
+	}
+
+	// Calculate coefficients.
+
+	// This check could possibly be removed as we dealt with this
+	// case in the find_method() function, but is left for safety, it probably
+	// isn't a bottleneck.
+	if ((1.0f - cosom) > (real_t)CMP_EPSILON) {
+		// standard case (slerp)
+		omega = Math::acos(cosom);
+		sinom = Math::sin(omega);
+		scale0 = Math::sin((1.0f - p_fraction) * omega) / sinom;
+		scale1 = Math::sin(p_fraction * omega) / sinom;
+	} else {
+		// "from" and "to" quaternions are very close
+		//  ... so we can do a linear interpolation
+		scale0 = 1.0f - p_fraction;
+		scale1 = p_fraction;
+	}
+	// Calculate final values.
+	return Quaternion(
+			scale0 * p_from.x + scale1 * to1.x,
+			scale0 * p_from.y + scale1 * to1.y,
+			scale0 * p_from.z + scale1 * to1.z,
+			scale0 * p_from.w + scale1 * to1.w);
+}
+
+Basis TransformInterpolator::_basis_slerp_unchecked(Basis p_from, Basis p_to, real_t p_fraction) {
+	Quaternion from = _basis_to_quat_unchecked(p_from);
+	Quaternion to = _basis_to_quat_unchecked(p_to);
+
+	Basis b(_quat_slerp_unchecked(from, to, p_fraction));
+	return b;
+}
+
+void TransformInterpolator::interpolate_basis_scaled_slerp(Basis p_prev, Basis p_curr, Basis &r_result, real_t p_fraction) {
+	// Normalize both and find lengths.
+	Vector3 lengths_prev = _basis_orthonormalize(p_prev);
+	Vector3 lengths_curr = _basis_orthonormalize(p_curr);
+
+	r_result = _basis_slerp_unchecked(p_prev, p_curr, p_fraction);
+
+	// Now the result is unit length basis, we need to scale.
+	Vector3 lengths_lerped = lengths_prev + ((lengths_curr - lengths_prev) * p_fraction);
+
+	// Keep a note that the column / row order of the basis is weird,
+	// so keep an eye for bugs with this.
+	r_result[0] *= lengths_lerped;
+	r_result[1] *= lengths_lerped;
+	r_result[2] *= lengths_lerped;
+}
+
+void TransformInterpolator::interpolate_basis_linear(const Basis &p_prev, const Basis &p_curr, Basis &r_result, real_t p_fraction) {
+	// Interpolate basis.
+	r_result = p_prev.lerp(p_curr, p_fraction);
+
+	// It turns out we need to guard against zero scale basis.
+	// This is kind of silly, as we should probably fix the bugs elsewhere in Godot that can't deal with
+	// zero scale, but until that time...
+	for (int n = 0; n < 3; n++) {
+		Vector3 &axis = r_result[n];
+
+		// Not ok, this could cause errors due to bugs elsewhere,
+		// so we will bodge set this to a small value.
+		const real_t smallest = 0.0001f;
+		const real_t smallest_squared = smallest * smallest;
+		if (axis.length_squared() < smallest_squared) {
+			// Setting a different component to the smallest
+			// helps prevent the situation where all the axes are pointing in the same direction,
+			// which could be a problem for e.g. cross products...
+			axis[n] = smallest;
+		}
+	}
+}
+
+// Returns length.
+real_t TransformInterpolator::_vec3_normalize(Vector3 &p_vec) {
+	real_t lengthsq = p_vec.length_squared();
+	if (lengthsq == 0.0f) {
+		p_vec.x = p_vec.y = p_vec.z = 0.0f;
+		return 0.0f;
+	}
+	real_t length = Math::sqrt(lengthsq);
+	p_vec.x /= length;
+	p_vec.y /= length;
+	p_vec.z /= length;
+	return length;
+}
+
+// Returns lengths.
+Vector3 TransformInterpolator::_basis_orthonormalize(Basis &r_basis) {
+	// Gram-Schmidt Process.
+
+	Vector3 x = r_basis.get_column(0);
+	Vector3 y = r_basis.get_column(1);
+	Vector3 z = r_basis.get_column(2);
+
+	Vector3 lengths;
+
+	lengths.x = _vec3_normalize(x);
+	y = (y - x * (x.dot(y)));
+	lengths.y = _vec3_normalize(y);
+	z = (z - x * (x.dot(z)) - y * (y.dot(z)));
+	lengths.z = _vec3_normalize(z);
+
+	r_basis.set_column(0, x);
+	r_basis.set_column(1, y);
+	r_basis.set_column(2, z);
+
+	return lengths;
+}
+
+TransformInterpolator::Method TransformInterpolator::_test_basis(Basis p_basis, bool r_needed_normalize, Quaternion &r_quat) {
+	// Axis lengths.
+	Vector3 al = Vector3(p_basis.get_column(0).length_squared(),
+			p_basis.get_column(1).length_squared(),
+			p_basis.get_column(2).length_squared());
+
+	// Non unit scale?
+	if (r_needed_normalize || !_vec3_is_equal_approx(al, Vector3(1.0, 1.0, 1.0), (real_t)0.001f)) {
+		// If the basis is not normalized (at least approximately), it will fail the checks needed for slerp.
+		// So we try to detect a scaled (but not sheared) basis, which we *can* slerp by normalizing first,
+		// and lerping the scales separately.
+
+		// If any of the axes are really small, it is unlikely to be a valid rotation, or is scaled too small to deal with float error.
+		const real_t sl_epsilon = 0.00001f;
+		if ((al.x < sl_epsilon) ||
+				(al.y < sl_epsilon) ||
+				(al.z < sl_epsilon)) {
+			return INTERP_LERP;
+		}
+
+		// Normalize the basis.
+		Basis norm_basis = p_basis;
+
+		al.x = Math::sqrt(al.x);
+		al.y = Math::sqrt(al.y);
+		al.z = Math::sqrt(al.z);
+
+		norm_basis.set_column(0, norm_basis.get_column(0) / al.x);
+		norm_basis.set_column(1, norm_basis.get_column(1) / al.y);
+		norm_basis.set_column(2, norm_basis.get_column(2) / al.z);
+
+		// This doesn't appear necessary, as the later checks will catch it.
+		// if (!_basis_is_orthogonal_any_scale(norm_basis)) {
+		// return INTERP_LERP;
+		// }
+
+		p_basis = norm_basis;
+
+		// Orthonormalize not necessary as normal normalization(!) works if the
+		// axes are orthonormal.
+		// p_basis.orthonormalize();
+
+		// If we needed to normalize one of the two bases, we will need to normalize both,
+		// regardless of whether the 2nd needs it, just to make sure it takes the path to return
+		// INTERP_SCALED_LERP on the 2nd call of _test_basis.
+		r_needed_normalize = true;
+	}
+
+	// Apply less stringent tests than the built in slerp, the standard Godot slerp
+	// is too susceptible to float error to be useful.
+	real_t det = p_basis.determinant();
+	if (!Math::is_equal_approx(det, 1, (real_t)0.01f)) {
+		return INTERP_LERP;
+	}
+
+	if (!_basis_is_orthogonal(p_basis)) {
+		return INTERP_LERP;
+	}
+
+	// TODO: This could possibly be less stringent too, check this.
+	r_quat = _basis_to_quat_unchecked(p_basis);
+	if (!r_quat.is_normalized()) {
+		return INTERP_LERP;
+	}
+
+	return r_needed_normalize ? INTERP_SCALED_SLERP : INTERP_SLERP;
+}
+
+// This check doesn't seem to be needed but is preserved in case of bugs.
+bool TransformInterpolator::_basis_is_orthogonal_any_scale(const Basis &p_basis) {
+	Vector3 cross = p_basis.get_column(0).cross(p_basis.get_column(1));
+	real_t l = _vec3_normalize(cross);
+	// Too small numbers, revert to lerp.
+	if (l < 0.001f) {
+		return false;
+	}
+
+	const real_t epsilon = 0.9995f;
+
+	real_t dot = cross.dot(p_basis.get_column(2));
+	if (dot < epsilon) {
+		return false;
+	}
+
+	cross = p_basis.get_column(1).cross(p_basis.get_column(2));
+	l = _vec3_normalize(cross);
+	// Too small numbers, revert to lerp.
+	if (l < 0.001f) {
+		return false;
+	}
+
+	dot = cross.dot(p_basis.get_column(0));
+	if (dot < epsilon) {
+		return false;
+	}
+
+	return true;
+}
+
+bool TransformInterpolator::_basis_is_orthogonal(const Basis &p_basis, real_t p_epsilon) {
+	Basis identity;
+	Basis m = p_basis * p_basis.transposed();
+
+	// Less stringent tests than the standard Godot slerp.
+	if (!_vec3_is_equal_approx(m[0], identity[0], p_epsilon) || !_vec3_is_equal_approx(m[1], identity[1], p_epsilon) || !_vec3_is_equal_approx(m[2], identity[2], p_epsilon)) {
+		return false;
+	}
+	return true;
+}
+
+real_t TransformInterpolator::checksum_transform_3d(const Transform3D &p_transform) {
+	// just a really basic checksum, this can probably be improved
+	real_t sum = _vec3_sum(p_transform.origin);
+	sum -= _vec3_sum(p_transform.basis.rows[0]);
+	sum += _vec3_sum(p_transform.basis.rows[1]);
+	sum -= _vec3_sum(p_transform.basis.rows[2]);
+	return sum;
+}
+
+TransformInterpolator::Method TransformInterpolator::find_method(const Basis &p_a, const Basis &p_b) {
+	bool needed_normalize = false;
+
+	Quaternion q0;
+	Method method = _test_basis(p_a, needed_normalize, q0);
+	if (method == INTERP_LERP) {
+		return method;
+	}
+
+	Quaternion q1;
+	method = _test_basis(p_b, needed_normalize, q1);
+	if (method == INTERP_LERP) {
+		return method;
+	}
+
+	// Are they close together?
+	// Apply the same test that will revert to lerp as is present in the slerp routine.
+	// Calculate cosine.
+	real_t cosom = Math::abs(q0.dot(q1));
+	if ((1.0f - cosom) <= (real_t)CMP_EPSILON) {
+		return INTERP_LERP;
+	}
+
+	return method;
+}

core/math/transform_interpolator.h

@@ -0,0 +1,92 @@
+/**************************************************************************/
+/*  transform_interpolator.h                                              */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "core/math/math_defs.h"
+#include "core/math/vector3.h"
+
+// Keep all the functions for fixed timestep interpolation together.
+// There are two stages involved:
+// Finding a method, for determining the interpolation method between two
+// keyframes (which are physics ticks).
+// And applying that pre-determined method.
+
+// Pre-determining the method makes sense because it is expensive and often
+// several frames may occur between each physics tick, which will make it cheaper
+// than performing every frame.
+
+struct Transform2D;
+struct Transform3D;
+struct Basis;
+struct Quaternion;
+
+class TransformInterpolator {
+public:
+	enum Method {
+		INTERP_LERP,
+		INTERP_SLERP,
+		INTERP_SCALED_SLERP,
+	};
+
+private:
+	_FORCE_INLINE_ static bool _sign(real_t p_val) { return p_val >= 0; }
+	static real_t _vec3_sum(const Vector3 &p_pt) { return p_pt.x + p_pt.y + p_pt.z; }
+	static real_t _vec3_normalize(Vector3 &p_vec);
+	_FORCE_INLINE_ static bool _vec3_is_equal_approx(const Vector3 &p_a, const Vector3 &p_b, real_t p_tolerance) {
+		return Math::is_equal_approx(p_a.x, p_b.x, p_tolerance) && Math::is_equal_approx(p_a.y, p_b.y, p_tolerance) && Math::is_equal_approx(p_a.z, p_b.z, p_tolerance);
+	}
+	static Vector3 _basis_orthonormalize(Basis &r_basis);
+	static Method _test_basis(Basis p_basis, bool r_needed_normalize, Quaternion &r_quat);
+	static Basis _basis_slerp_unchecked(Basis p_from, Basis p_to, real_t p_fraction);
+	static Quaternion _quat_slerp_unchecked(const Quaternion &p_from, const Quaternion &p_to, real_t p_fraction);
+	static Quaternion _basis_to_quat_unchecked(const Basis &p_basis);
+	static bool _basis_is_orthogonal(const Basis &p_basis, real_t p_epsilon = 0.01f);
+	static bool _basis_is_orthogonal_any_scale(const Basis &p_basis);
+
+	static void interpolate_basis_linear(const Basis &p_prev, const Basis &p_curr, Basis &r_result, real_t p_fraction);
+	static void interpolate_basis_scaled_slerp(Basis p_prev, Basis p_curr, Basis &r_result, real_t p_fraction);
+
+public:
+	static void interpolate_transform_2d(const Transform2D &p_prev, const Transform2D &p_curr, Transform2D &r_result, real_t p_fraction);
+
+	// Generic functions, use when you don't know what method should be used, e.g. from GDScript.
+	// These will be slower.
+	static void interpolate_transform_3d(const Transform3D &p_prev, const Transform3D &p_curr, Transform3D &r_result, real_t p_fraction);
+	static void interpolate_basis(const Basis &p_prev, const Basis &p_curr, Basis &r_result, real_t p_fraction);
+
+	// Optimized function when you know ahead of time the method.
+	static void interpolate_transform_3d_via_method(const Transform3D &p_prev, const Transform3D &p_curr, Transform3D &r_result, real_t p_fraction, Method p_method);
+	static void interpolate_basis_via_method(const Basis &p_prev, const Basis &p_curr, Basis &r_result, real_t p_fraction, Method p_method);
+
+	static real_t checksum_transform_3d(const Transform3D &p_transform);
+
+	static Method find_method(const Basis &p_a, const Basis &p_b);
+};

core/math/triangle_mesh.cpp

@@ -0,0 +1,606 @@
+/**************************************************************************/
+/*  triangle_mesh.cpp                                                     */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "triangle_mesh.h"
+
+#include "core/templates/sort_array.h"
+
+int TriangleMesh::_create_bvh(BVH *p_bvh, BVH **p_bb, int p_from, int p_size, int p_depth, int &r_max_depth, int &r_max_alloc) {
+	if (p_depth > r_max_depth) {
+		r_max_depth = p_depth;
+	}
+
+	if (p_size == 1) {
+		return p_bb[p_from] - p_bvh;
+	} else if (p_size == 0) {
+		return -1;
+	}
+
+	AABB aabb;
+	aabb = p_bb[p_from]->aabb;
+	for (int i = 1; i < p_size; i++) {
+		aabb.merge_with(p_bb[p_from + i]->aabb);
+	}
+
+	int li = aabb.get_longest_axis_index();
+
+	switch (li) {
+		case Vector3::AXIS_X: {
+			SortArray<BVH *, BVHCmpX> sort_x;
+			sort_x.nth_element(0, p_size, p_size / 2, &p_bb[p_from]);
+			//sort_x.sort(&p_bb[p_from],p_size);
+		} break;
+		case Vector3::AXIS_Y: {
+			SortArray<BVH *, BVHCmpY> sort_y;
+			sort_y.nth_element(0, p_size, p_size / 2, &p_bb[p_from]);
+			//sort_y.sort(&p_bb[p_from],p_size);
+		} break;
+		case Vector3::AXIS_Z: {
+			SortArray<BVH *, BVHCmpZ> sort_z;
+			sort_z.nth_element(0, p_size, p_size / 2, &p_bb[p_from]);
+			//sort_z.sort(&p_bb[p_from],p_size);
+
+		} break;
+	}
+
+	int left = _create_bvh(p_bvh, p_bb, p_from, p_size / 2, p_depth + 1, r_max_depth, r_max_alloc);
+	int right = _create_bvh(p_bvh, p_bb, p_from + p_size / 2, p_size - p_size / 2, p_depth + 1, r_max_depth, r_max_alloc);
+
+	int index = r_max_alloc++;
+	BVH *_new = &p_bvh[index];
+	_new->aabb = aabb;
+	_new->center = aabb.get_center();
+	_new->face_index = -1;
+	_new->left = left;
+	_new->right = right;
+
+	return index;
+}
+
+void TriangleMesh::get_indices(Vector<int> *r_triangles_indices) const {
+	if (!valid) {
+		return;
+	}
+
+	const int triangles_num = triangles.size();
+
+	// Parse vertices indices
+	const Triangle *triangles_read = triangles.ptr();
+
+	r_triangles_indices->resize(triangles_num * 3);
+	int *r_indices_write = r_triangles_indices->ptrw();
+
+	for (int i = 0; i < triangles_num; ++i) {
+		r_indices_write[3 * i + 0] = triangles_read[i].indices[0];
+		r_indices_write[3 * i + 1] = triangles_read[i].indices[1];
+		r_indices_write[3 * i + 2] = triangles_read[i].indices[2];
+	}
+}
+
+void TriangleMesh::create(const Vector<Vector3> &p_faces, const Vector<int32_t> &p_surface_indices) {
+	valid = false;
+
+	ERR_FAIL_COND(p_surface_indices.size() && p_surface_indices.size() != p_faces.size());
+
+	int fc = p_faces.size();
+	ERR_FAIL_COND(!fc || ((fc % 3) != 0));
+	fc /= 3;
+	triangles.resize(fc);
+
+	bvh.resize(fc * 3); //will never be larger than this (todo make better)
+	BVH *bw = bvh.ptrw();
+
+	{
+		//create faces and indices and base bvh
+		//except for the Set for repeated triangles, everything
+		//goes in-place.
+
+		const Vector3 *r = p_faces.ptr();
+		const int32_t *si = p_surface_indices.ptr();
+		Triangle *w = triangles.ptrw();
+		HashMap<Vector3, int> db;
+
+		for (int i = 0; i < fc; i++) {
+			Triangle &f = w[i];
+			const Vector3 *v = &r[i * 3];
+
+			for (int j = 0; j < 3; j++) {
+				int vidx = -1;
+				Vector3 vs = v[j].snappedf(0.0001);
+				HashMap<Vector3, int>::Iterator E = db.find(vs);
+				if (E) {
+					vidx = E->value;
+				} else {
+					vidx = db.size();
+					db[vs] = vidx;
+				}
+
+				f.indices[j] = vidx;
+				if (j == 0) {
+					bw[i].aabb.position = vs;
+				} else {
+					bw[i].aabb.expand_to(vs);
+				}
+			}
+
+			f.normal = Face3(r[i * 3 + 0], r[i * 3 + 1], r[i * 3 + 2]).get_plane().get_normal();
+			f.surface_index = si ? si[i] : 0;
+
+			bw[i].left = -1;
+			bw[i].right = -1;
+			bw[i].face_index = i;
+			bw[i].center = bw[i].aabb.get_center();
+		}
+
+		vertices.resize(db.size());
+		Vector3 *vw = vertices.ptrw();
+		for (const KeyValue<Vector3, int> &E : db) {
+			vw[E.value] = E.key;
+		}
+	}
+
+	Vector<BVH *> bwptrs;
+	bwptrs.resize(fc);
+	BVH **bwp = bwptrs.ptrw();
+	for (int i = 0; i < fc; i++) {
+		bwp[i] = &bw[i];
+	}
+
+	max_depth = 0;
+	int max_alloc = fc;
+	_create_bvh(bw, bwp, 0, fc, 1, max_depth, max_alloc);
+
+	bvh.resize(max_alloc); //resize back
+
+	valid = true;
+}
+
+bool TriangleMesh::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_point, Vector3 &r_normal, int32_t *r_surf_index, int32_t *r_face_index) const {
+	if (!valid) {
+		return false;
+	}
+
+	uint32_t *stack = (uint32_t *)alloca(sizeof(int) * max_depth);
+
+	enum {
+		TEST_AABB_BIT = 0,
+		VISIT_LEFT_BIT = 1,
+		VISIT_RIGHT_BIT = 2,
+		VISIT_DONE_BIT = 3,
+		VISITED_BIT_SHIFT = 29,
+		NODE_IDX_MASK = (1 << VISITED_BIT_SHIFT) - 1,
+		VISITED_BIT_MASK = ~NODE_IDX_MASK,
+
+	};
+
+	Vector3 n = (p_end - p_begin).normalized();
+	real_t d = 1e10;
+	bool inters = false;
+
+	int level = 0;
+
+	const Triangle *triangleptr = triangles.ptr();
+	const Vector3 *vertexptr = vertices.ptr();
+	const BVH *bvhptr = bvh.ptr();
+
+	int pos = bvh.size() - 1;
+
+	stack[0] = pos;
+	while (true) {
+		uint32_t node = stack[level] & NODE_IDX_MASK;
+		const BVH &b = bvhptr[node];
+		bool done = false;
+
+		switch (stack[level] >> VISITED_BIT_SHIFT) {
+			case TEST_AABB_BIT: {
+				if (!b.aabb.intersects_segment(p_begin, p_end)) {
+					stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node;
+				} else {
+					if (b.face_index >= 0) {
+						const Triangle &s = triangleptr[b.face_index];
+						Face3 f3(vertexptr[s.indices[0]], vertexptr[s.indices[1]], vertexptr[s.indices[2]]);
+
+						Vector3 res;
+
+						if (f3.intersects_segment(p_begin, p_end, &res)) {
+							real_t nd = n.dot(res);
+							if (nd < d) {
+								d = nd;
+								r_point = res;
+								r_normal = f3.get_plane().get_normal();
+								if (r_surf_index) {
+									*r_surf_index = s.surface_index;
+								}
+								if (r_face_index) {
+									*r_face_index = b.face_index;
+								}
+								inters = true;
+							}
+						}
+
+						stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node;
+
+					} else {
+						stack[level] = (VISIT_LEFT_BIT << VISITED_BIT_SHIFT) | node;
+					}
+				}
+				continue;
+			}
+			case VISIT_LEFT_BIT: {
+				stack[level] = (VISIT_RIGHT_BIT << VISITED_BIT_SHIFT) | node;
+				level++;
+				stack[level] = b.left | TEST_AABB_BIT;
+				continue;
+			}
+			case VISIT_RIGHT_BIT: {
+				stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node;
+				level++;
+				stack[level] = b.right | TEST_AABB_BIT;
+				continue;
+			}
+			case VISIT_DONE_BIT: {
+				if (level == 0) {
+					done = true;
+					break;
+				} else {
+					level--;
+				}
+				continue;
+			}
+		}
+
+		if (done) {
+			break;
+		}
+	}
+
+	if (inters) {
+		if (n.dot(r_normal) > 0) {
+			r_normal = -r_normal;
+		}
+	}
+
+	return inters;
+}
+
+bool TriangleMesh::intersect_ray(const Vector3 &p_begin, const Vector3 &p_dir, Vector3 &r_point, Vector3 &r_normal, int32_t *r_surf_index, int32_t *r_face_index) const {
+	if (!valid) {
+		return false;
+	}
+
+	uint32_t *stack = (uint32_t *)alloca(sizeof(int) * max_depth);
+
+	enum {
+		TEST_AABB_BIT = 0,
+		VISIT_LEFT_BIT = 1,
+		VISIT_RIGHT_BIT = 2,
+		VISIT_DONE_BIT = 3,
+		VISITED_BIT_SHIFT = 29,
+		NODE_IDX_MASK = (1 << VISITED_BIT_SHIFT) - 1,
+		VISITED_BIT_MASK = ~NODE_IDX_MASK,
+
+	};
+
+	Vector3 n = p_dir;
+	real_t d = 1e20;
+	bool inters = false;
+
+	int level = 0;
+
+	const Triangle *triangleptr = triangles.ptr();
+	const Vector3 *vertexptr = vertices.ptr();
+	const BVH *bvhptr = bvh.ptr();
+
+	int pos = bvh.size() - 1;
+
+	stack[0] = pos;
+	while (true) {
+		uint32_t node = stack[level] & NODE_IDX_MASK;
+		const BVH &b = bvhptr[node];
+		bool done = false;
+
+		switch (stack[level] >> VISITED_BIT_SHIFT) {
+			case TEST_AABB_BIT: {
+				if (!b.aabb.intersects_ray(p_begin, p_dir)) {
+					stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node;
+				} else {
+					if (b.face_index >= 0) {
+						const Triangle &s = triangleptr[b.face_index];
+						Face3 f3(vertexptr[s.indices[0]], vertexptr[s.indices[1]], vertexptr[s.indices[2]]);
+
+						Vector3 res;
+
+						if (f3.intersects_ray(p_begin, p_dir, &res)) {
+							real_t nd = n.dot(res);
+							if (nd < d) {
+								d = nd;
+								r_point = res;
+								r_normal = f3.get_plane().get_normal();
+								if (r_surf_index) {
+									*r_surf_index = s.surface_index;
+								}
+								if (r_face_index) {
+									*r_face_index = b.face_index;
+								}
+								inters = true;
+							}
+						}
+
+						stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node;
+
+					} else {
+						stack[level] = (VISIT_LEFT_BIT << VISITED_BIT_SHIFT) | node;
+					}
+				}
+				continue;
+			}
+			case VISIT_LEFT_BIT: {
+				stack[level] = (VISIT_RIGHT_BIT << VISITED_BIT_SHIFT) | node;
+				level++;
+				stack[level] = b.left | TEST_AABB_BIT;
+				continue;
+			}
+			case VISIT_RIGHT_BIT: {
+				stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node;
+				level++;
+				stack[level] = b.right | TEST_AABB_BIT;
+				continue;
+			}
+			case VISIT_DONE_BIT: {
+				if (level == 0) {
+					done = true;
+					break;
+				} else {
+					level--;
+				}
+				continue;
+			}
+		}
+
+		if (done) {
+			break;
+		}
+	}
+
+	if (inters) {
+		if (n.dot(r_normal) > 0) {
+			r_normal = -r_normal;
+		}
+	}
+
+	return inters;
+}
+
+bool TriangleMesh::inside_convex_shape(const Plane *p_planes, int p_plane_count, const Vector3 *p_points, int p_point_count, Vector3 p_scale) const {
+	if (!valid) {
+		return false;
+	}
+
+	uint32_t *stack = (uint32_t *)alloca(sizeof(int) * max_depth);
+
+	enum {
+		TEST_AABB_BIT = 0,
+		VISIT_LEFT_BIT = 1,
+		VISIT_RIGHT_BIT = 2,
+		VISIT_DONE_BIT = 3,
+		VISITED_BIT_SHIFT = 29,
+		NODE_IDX_MASK = (1 << VISITED_BIT_SHIFT) - 1,
+		VISITED_BIT_MASK = ~NODE_IDX_MASK,
+
+	};
+
+	int level = 0;
+
+	const Triangle *triangleptr = triangles.ptr();
+	const Vector3 *vertexptr = vertices.ptr();
+	const BVH *bvhptr = bvh.ptr();
+
+	Transform3D scale(Basis().scaled(p_scale));
+
+	int pos = bvh.size() - 1;
+
+	stack[0] = pos;
+	while (true) {
+		uint32_t node = stack[level] & NODE_IDX_MASK;
+		const BVH &b = bvhptr[node];
+		bool done = false;
+
+		switch (stack[level] >> VISITED_BIT_SHIFT) {
+			case TEST_AABB_BIT: {
+				bool intersects = scale.xform(b.aabb).intersects_convex_shape(p_planes, p_plane_count, p_points, p_point_count);
+				if (!intersects) {
+					return false;
+				}
+
+				bool inside = scale.xform(b.aabb).inside_convex_shape(p_planes, p_plane_count);
+				if (inside) {
+					stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node;
+
+				} else {
+					if (b.face_index >= 0) {
+						const Triangle &s = triangleptr[b.face_index];
+						for (int j = 0; j < 3; ++j) {
+							Vector3 point = scale.xform(vertexptr[s.indices[j]]);
+							for (int i = 0; i < p_plane_count; i++) {
+								const Plane &p = p_planes[i];
+								if (p.is_point_over(point)) {
+									return false;
+								}
+							}
+						}
+
+						stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node;
+
+					} else {
+						stack[level] = (VISIT_LEFT_BIT << VISITED_BIT_SHIFT) | node;
+					}
+				}
+				continue;
+			}
+			case VISIT_LEFT_BIT: {
+				stack[level] = (VISIT_RIGHT_BIT << VISITED_BIT_SHIFT) | node;
+				level++;
+				stack[level] = b.left | TEST_AABB_BIT;
+				continue;
+			}
+			case VISIT_RIGHT_BIT: {
+				stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node;
+				level++;
+				stack[level] = b.right | TEST_AABB_BIT;
+				continue;
+			}
+			case VISIT_DONE_BIT: {
+				if (level == 0) {
+					done = true;
+					break;
+				} else {
+					level--;
+				}
+				continue;
+			}
+		}
+
+		if (done) {
+			break;
+		}
+	}
+
+	return true;
+}
+
+bool TriangleMesh::is_valid() const {
+	return valid;
+}
+
+Vector<Face3> TriangleMesh::get_faces() const {
+	if (!valid) {
+		return Vector<Face3>();
+	}
+
+	Vector<Face3> faces;
+	int ts = triangles.size();
+	faces.resize(triangles.size());
+
+	Face3 *w = faces.ptrw();
+	const Triangle *r = triangles.ptr();
+	const Vector3 *rv = vertices.ptr();
+
+	for (int i = 0; i < ts; i++) {
+		for (int j = 0; j < 3; j++) {
+			w[i].vertex[j] = rv[r[i].indices[j]];
+		}
+	}
+
+	return faces;
+}
+
+bool TriangleMesh::create_from_faces(const Vector<Vector3> &p_faces) {
+	create(p_faces);
+	return is_valid();
+}
+
+Dictionary TriangleMesh::intersect_segment_scriptwrap(const Vector3 &p_begin, const Vector3 &p_end) const {
+	if (!valid) {
+		return Dictionary();
+	}
+
+	Vector3 r_point;
+	Vector3 r_normal;
+	int32_t r_face_index = -1;
+
+	bool intersected = intersect_segment(p_begin, p_end, r_point, r_normal, nullptr, &r_face_index);
+	if (!intersected) {
+		return Dictionary();
+	}
+
+	Dictionary result;
+	result["position"] = r_point;
+	result["normal"] = r_normal;
+	result["face_index"] = r_face_index;
+
+	return result;
+}
+
+Dictionary TriangleMesh::intersect_ray_scriptwrap(const Vector3 &p_begin, const Vector3 &p_dir) const {
+	if (!valid) {
+		return Dictionary();
+	}
+
+	Vector3 r_point;
+	Vector3 r_normal;
+	int32_t r_face_index = -1;
+
+	bool intersected = intersect_ray(p_begin, p_dir, r_point, r_normal, nullptr, &r_face_index);
+	if (!intersected) {
+		return Dictionary();
+	}
+
+	Dictionary result;
+	result["position"] = r_point;
+	result["normal"] = r_normal;
+	result["face_index"] = r_face_index;
+
+	return result;
+}
+
+Vector<Vector3> TriangleMesh::get_faces_scriptwrap() const {
+	if (!valid) {
+		return Vector<Vector3>();
+	}
+
+	Vector<Vector3> faces;
+	int ts = triangles.size();
+	faces.resize(triangles.size() * 3);
+
+	Vector3 *w = faces.ptrw();
+	const Triangle *r = triangles.ptr();
+	const Vector3 *rv = vertices.ptr();
+
+	for (int i = 0; i < ts; i++) {
+		for (int j = 0; j < 3; j++) {
+			w[i * 3 + j] = rv[r[i].indices[j]];
+		}
+	}
+
+	return faces;
+}
+
+void TriangleMesh::_bind_methods() {
+	ClassDB::bind_method(D_METHOD("create_from_faces", "faces"), &TriangleMesh::create_from_faces);
+	ClassDB::bind_method(D_METHOD("get_faces"), &TriangleMesh::get_faces_scriptwrap);
+
+	ClassDB::bind_method(D_METHOD("intersect_segment", "begin", "end"), &TriangleMesh::intersect_segment_scriptwrap);
+	ClassDB::bind_method(D_METHOD("intersect_ray", "begin", "dir"), &TriangleMesh::intersect_ray_scriptwrap);
+}
+
+TriangleMesh::TriangleMesh() {
+	valid = false;
+	max_depth = 0;
+}

core/math/triangle_mesh.h

@@ -0,0 +1,106 @@
+/**************************************************************************/
+/*  triangle_mesh.h                                                       */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "core/math/face3.h"
+#include "core/object/ref_counted.h"
+
+class TriangleMesh : public RefCounted {
+	GDCLASS(TriangleMesh, RefCounted);
+
+public:
+	struct Triangle {
+		Vector3 normal;
+		int indices[3];
+		int32_t surface_index = 0;
+	};
+
+protected:
+	static void _bind_methods();
+
+private:
+	Vector<Triangle> triangles;
+	Vector<Vector3> vertices;
+
+	struct BVH {
+		AABB aabb;
+		Vector3 center; //used for sorting
+		int left = -1;
+		int right = -1;
+
+		int32_t face_index = -1;
+	};
+
+	struct BVHCmpX {
+		bool operator()(const BVH *p_left, const BVH *p_right) const {
+			return p_left->center.x < p_right->center.x;
+		}
+	};
+
+	struct BVHCmpY {
+		bool operator()(const BVH *p_left, const BVH *p_right) const {
+			return p_left->center.y < p_right->center.y;
+		}
+	};
+	struct BVHCmpZ {
+		bool operator()(const BVH *p_left, const BVH *p_right) const {
+			return p_left->center.z < p_right->center.z;
+		}
+	};
+
+	int _create_bvh(BVH *p_bvh, BVH **p_bb, int p_from, int p_size, int p_depth, int &max_depth, int &max_alloc);
+
+	Vector<BVH> bvh;
+	int max_depth = 0;
+	bool valid = false;
+
+public:
+	bool is_valid() const;
+	bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_point, Vector3 &r_normal, int32_t *r_surf_index = nullptr, int32_t *r_face_index = nullptr) const;
+	bool intersect_ray(const Vector3 &p_begin, const Vector3 &p_dir, Vector3 &r_point, Vector3 &r_normal, int32_t *r_surf_index = nullptr, int32_t *r_face_index = nullptr) const;
+	bool inside_convex_shape(const Plane *p_planes, int p_plane_count, const Vector3 *p_points, int p_point_count, Vector3 p_scale = Vector3(1, 1, 1)) const;
+	Vector<Face3> get_faces() const;
+
+	const Vector<Triangle> &get_triangles() const { return triangles; }
+	const Vector<Vector3> &get_vertices() const { return vertices; }
+	void get_indices(Vector<int> *r_triangles_indices) const;
+
+	void create(const Vector<Vector3> &p_faces, const Vector<int32_t> &p_surface_indices = Vector<int32_t>());
+
+	// Wrapped functions for compatibility with method bindings
+	// and user exposed script api that can't use more native types.
+	bool create_from_faces(const Vector<Vector3> &p_faces);
+	Dictionary intersect_segment_scriptwrap(const Vector3 &p_begin, const Vector3 &p_end) const;
+	Dictionary intersect_ray_scriptwrap(const Vector3 &p_begin, const Vector3 &p_dir) const;
+	Vector<Vector3> get_faces_scriptwrap() const;
+
+	TriangleMesh();
+};

core/math/triangulate.cpp

@@ -0,0 +1,207 @@
+/**************************************************************************/
+/*  triangulate.cpp                                                       */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "triangulate.h"
+
+real_t Triangulate::get_area(const Vector<Vector2> &contour) {
+	int n = contour.size();
+	const Vector2 *c = &contour[0];
+
+	real_t A = 0.0;
+
+	for (int p = n - 1, q = 0; q < n; p = q++) {
+		A += c[p].cross(c[q]);
+	}
+	return A * 0.5f;
+}
+
+/* `is_inside_triangle` decides if a point P is inside the triangle
+ * defined by A, B, C. */
+bool Triangulate::is_inside_triangle(real_t Ax, real_t Ay,
+		real_t Bx, real_t By,
+		real_t Cx, real_t Cy,
+		real_t Px, real_t Py,
+		bool include_edges) {
+	real_t ax, ay, bx, by, cx, cy, apx, apy, bpx, bpy, cpx, cpy;
+	real_t cCROSSap, bCROSScp, aCROSSbp;
+
+	ax = Cx - Bx;
+	ay = Cy - By;
+	bx = Ax - Cx;
+	by = Ay - Cy;
+	cx = Bx - Ax;
+	cy = By - Ay;
+	apx = Px - Ax;
+	apy = Py - Ay;
+	bpx = Px - Bx;
+	bpy = Py - By;
+	cpx = Px - Cx;
+	cpy = Py - Cy;
+
+	aCROSSbp = ax * bpy - ay * bpx;
+	cCROSSap = cx * apy - cy * apx;
+	bCROSScp = bx * cpy - by * cpx;
+
+	if (include_edges) {
+		return ((aCROSSbp > 0.0f) && (bCROSScp > 0.0f) && (cCROSSap > 0.0f));
+	} else {
+		return ((aCROSSbp >= 0.0f) && (bCROSScp >= 0.0f) && (cCROSSap >= 0.0f));
+	}
+}
+
+bool Triangulate::snip(const Vector<Vector2> &p_contour, int u, int v, int w, int n, const Vector<int> &V, bool relaxed) {
+	int p;
+	real_t Ax, Ay, Bx, By, Cx, Cy, Px, Py;
+	const Vector2 *contour = &p_contour[0];
+
+	Ax = contour[V[u]].x;
+	Ay = contour[V[u]].y;
+
+	Bx = contour[V[v]].x;
+	By = contour[V[v]].y;
+
+	Cx = contour[V[w]].x;
+	Cy = contour[V[w]].y;
+
+	// It can happen that the triangulation ends up with three aligned vertices to deal with.
+	// In this scenario, making the check below strict may reject the possibility of
+	// forming a last triangle with these aligned vertices, preventing the triangulation
+	// from completing.
+	// To avoid that we allow zero-area triangles if all else failed.
+	float threshold = relaxed ? -CMP_EPSILON : CMP_EPSILON;
+
+	if (threshold > (((Bx - Ax) * (Cy - Ay)) - ((By - Ay) * (Cx - Ax)))) {
+		return false;
+	}
+
+	for (p = 0; p < n; p++) {
+		if ((p == u) || (p == v) || (p == w)) {
+			continue;
+		}
+		Px = contour[V[p]].x;
+		Py = contour[V[p]].y;
+		if (is_inside_triangle(Ax, Ay, Bx, By, Cx, Cy, Px, Py, relaxed)) {
+			return false;
+		}
+	}
+
+	return true;
+}
+
+bool Triangulate::triangulate(const Vector<Vector2> &contour, Vector<int> &result) {
+	/* allocate and initialize list of Vertices in polygon */
+
+	int n = contour.size();
+	if (n < 3) {
+		return false;
+	}
+
+	Vector<int> V;
+	V.resize(n);
+
+	/* we want a counter-clockwise polygon in V */
+
+	if (0.0f < get_area(contour)) {
+		for (int v = 0; v < n; v++) {
+			V.write[v] = v;
+		}
+	} else {
+		for (int v = 0; v < n; v++) {
+			V.write[v] = (n - 1) - v;
+		}
+	}
+
+	bool relaxed = false;
+
+	int nv = n;
+
+	/*  remove nv-2 Vertices, creating 1 triangle every time */
+	int count = 2 * nv; /* error detection */
+
+	for (int v = nv - 1; nv > 2;) {
+		/* if we loop, it is probably a non-simple polygon */
+		if (0 >= (count--)) {
+			if (relaxed) {
+				//** Triangulate: ERROR - probable bad polygon!
+				return false;
+			} else {
+				// There may be aligned vertices that the strict
+				// checks prevent from triangulating. In this situation
+				// we are better off adding flat triangles than
+				// failing, so we relax the checks and try one last
+				// round.
+				// Only relaxing the constraints as a last resort avoids
+				// degenerate triangles when they aren't necessary.
+				count = 2 * nv;
+				relaxed = true;
+			}
+		}
+
+		/* three consecutive vertices in current polygon, <u,v,w> */
+		int u = v;
+		if (nv <= u) {
+			u = 0; /* previous */
+		}
+		v = u + 1;
+		if (nv <= v) {
+			v = 0; /* new v    */
+		}
+		int w = v + 1;
+		if (nv <= w) {
+			w = 0; /* next     */
+		}
+
+		if (snip(contour, u, v, w, nv, V, relaxed)) {
+			int a, b, c, s, t;
+
+			/* true names of the vertices */
+			a = V[u];
+			b = V[v];
+			c = V[w];
+
+			/* output Triangle */
+			result.push_back(a);
+			result.push_back(b);
+			result.push_back(c);
+
+			/* remove v from remaining polygon */
+			for (s = v, t = v + 1; t < nv; s++, t++) {
+				V.write[s] = V[t];
+			}
+
+			nv--;
+
+			/* reset error detection counter */
+			count = 2 * nv;
+		}
+	}
+
+	return true;
+}

core/math/triangulate.h

@@ -0,0 +1,59 @@
+/**************************************************************************/
+/*  triangulate.h                                                         */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "core/math/vector2.h"
+#include "core/templates/vector.h"
+
+/*
+https://www.flipcode.com/archives/Efficient_Polygon_Triangulation.shtml
+*/
+
+class Triangulate {
+public:
+	// triangulate a contour/polygon, places results in STL vector
+	// as series of triangles.
+	static bool triangulate(const Vector<Vector2> &contour, Vector<int> &result);
+
+	// compute area of a contour/polygon
+	static real_t get_area(const Vector<Vector2> &contour);
+
+	// decide if point Px/Py is inside triangle defined by
+	// (Ax,Ay) (Bx,By) (Cx,Cy)
+	static bool is_inside_triangle(real_t Ax, real_t Ay,
+			real_t Bx, real_t By,
+			real_t Cx, real_t Cy,
+			real_t Px, real_t Py,
+			bool include_edges);
+
+private:
+	static bool snip(const Vector<Vector2> &p_contour, int u, int v, int w, int n, const Vector<int> &V, bool relaxed);
+};

core/math/vector2.cpp

@@ -0,0 +1,215 @@
+/**************************************************************************/
+/*  vector2.cpp                                                           */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "vector2.h"
+
+#include "core/math/vector2i.h"
+#include "core/string/ustring.h"
+
+real_t Vector2::angle() const {
+	return Math::atan2(y, x);
+}
+
+Vector2 Vector2::from_angle(real_t p_angle) {
+	return Vector2(Math::cos(p_angle), Math::sin(p_angle));
+}
+
+real_t Vector2::length() const {
+	return Math::sqrt(x * x + y * y);
+}
+
+real_t Vector2::length_squared() const {
+	return x * x + y * y;
+}
+
+void Vector2::normalize() {
+	real_t l = x * x + y * y;
+	if (l != 0) {
+		l = Math::sqrt(l);
+		x /= l;
+		y /= l;
+	}
+}
+
+Vector2 Vector2::normalized() const {
+	Vector2 v = *this;
+	v.normalize();
+	return v;
+}
+
+bool Vector2::is_normalized() const {
+	// use length_squared() instead of length() to avoid sqrt(), makes it more stringent.
+	return Math::is_equal_approx(length_squared(), 1, (real_t)UNIT_EPSILON);
+}
+
+real_t Vector2::distance_to(const Vector2 &p_vector2) const {
+	return Math::sqrt((x - p_vector2.x) * (x - p_vector2.x) + (y - p_vector2.y) * (y - p_vector2.y));
+}
+
+real_t Vector2::distance_squared_to(const Vector2 &p_vector2) const {
+	return (x - p_vector2.x) * (x - p_vector2.x) + (y - p_vector2.y) * (y - p_vector2.y);
+}
+
+real_t Vector2::angle_to(const Vector2 &p_vector2) const {
+	return Math::atan2(cross(p_vector2), dot(p_vector2));
+}
+
+real_t Vector2::angle_to_point(const Vector2 &p_vector2) const {
+	return (p_vector2 - *this).angle();
+}
+
+real_t Vector2::dot(const Vector2 &p_other) const {
+	return x * p_other.x + y * p_other.y;
+}
+
+real_t Vector2::cross(const Vector2 &p_other) const {
+	return x * p_other.y - y * p_other.x;
+}
+
+Vector2 Vector2::sign() const {
+	return Vector2(SIGN(x), SIGN(y));
+}
+
+Vector2 Vector2::floor() const {
+	return Vector2(Math::floor(x), Math::floor(y));
+}
+
+Vector2 Vector2::ceil() const {
+	return Vector2(Math::ceil(x), Math::ceil(y));
+}
+
+Vector2 Vector2::round() const {
+	return Vector2(Math::round(x), Math::round(y));
+}
+
+Vector2 Vector2::rotated(real_t p_by) const {
+	real_t sine = Math::sin(p_by);
+	real_t cosi = Math::cos(p_by);
+	return Vector2(
+			x * cosi - y * sine,
+			x * sine + y * cosi);
+}
+
+Vector2 Vector2::posmod(real_t p_mod) const {
+	return Vector2(Math::fposmod(x, p_mod), Math::fposmod(y, p_mod));
+}
+
+Vector2 Vector2::posmodv(const Vector2 &p_modv) const {
+	return Vector2(Math::fposmod(x, p_modv.x), Math::fposmod(y, p_modv.y));
+}
+
+Vector2 Vector2::project(const Vector2 &p_to) const {
+	return p_to * (dot(p_to) / p_to.length_squared());
+}
+
+Vector2 Vector2::clamp(const Vector2 &p_min, const Vector2 &p_max) const {
+	return Vector2(
+			CLAMP(x, p_min.x, p_max.x),
+			CLAMP(y, p_min.y, p_max.y));
+}
+
+Vector2 Vector2::clampf(real_t p_min, real_t p_max) const {
+	return Vector2(
+			CLAMP(x, p_min, p_max),
+			CLAMP(y, p_min, p_max));
+}
+
+Vector2 Vector2::snapped(const Vector2 &p_step) const {
+	return Vector2(
+			Math::snapped(x, p_step.x),
+			Math::snapped(y, p_step.y));
+}
+
+Vector2 Vector2::snappedf(real_t p_step) const {
+	return Vector2(
+			Math::snapped(x, p_step),
+			Math::snapped(y, p_step));
+}
+
+Vector2 Vector2::limit_length(real_t p_len) const {
+	const real_t l = length();
+	Vector2 v = *this;
+	if (l > 0 && p_len < l) {
+		v /= l;
+		v *= p_len;
+	}
+
+	return v;
+}
+
+Vector2 Vector2::move_toward(const Vector2 &p_to, real_t p_delta) const {
+	Vector2 v = *this;
+	Vector2 vd = p_to - v;
+	real_t len = vd.length();
+	return len <= p_delta || len < (real_t)CMP_EPSILON ? p_to : v + vd / len * p_delta;
+}
+
+// slide returns the component of the vector along the given plane, specified by its normal vector.
+Vector2 Vector2::slide(const Vector2 &p_normal) const {
+#ifdef MATH_CHECKS
+	ERR_FAIL_COND_V_MSG(!p_normal.is_normalized(), Vector2(), "The normal Vector2 " + p_normal.operator String() + "must be normalized.");
+#endif
+	return *this - p_normal * dot(p_normal);
+}
+
+Vector2 Vector2::bounce(const Vector2 &p_normal) const {
+	return -reflect(p_normal);
+}
+
+Vector2 Vector2::reflect(const Vector2 &p_normal) const {
+#ifdef MATH_CHECKS
+	ERR_FAIL_COND_V_MSG(!p_normal.is_normalized(), Vector2(), "The normal Vector2 " + p_normal.operator String() + "must be normalized.");
+#endif
+	return 2.0f * p_normal * dot(p_normal) - *this;
+}
+
+bool Vector2::is_equal_approx(const Vector2 &p_v) const {
+	return Math::is_equal_approx(x, p_v.x) && Math::is_equal_approx(y, p_v.y);
+}
+
+bool Vector2::is_same(const Vector2 &p_v) const {
+	return Math::is_same(x, p_v.x) && Math::is_same(y, p_v.y);
+}
+
+bool Vector2::is_zero_approx() const {
+	return Math::is_zero_approx(x) && Math::is_zero_approx(y);
+}
+
+bool Vector2::is_finite() const {
+	return Math::is_finite(x) && Math::is_finite(y);
+}
+
+Vector2::operator String() const {
+	return "(" + String::num_real(x, true) + ", " + String::num_real(y, true) + ")";
+}
+
+Vector2::operator Vector2i() const {
+	return Vector2i(x, y);
+}

core/math/vector2.h

@@ -0,0 +1,333 @@
+/**************************************************************************/
+/*  vector2.h                                                             */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "core/error/error_macros.h"
+#include "core/math/math_funcs.h"
+
+class String;
+struct Vector2i;
+
+struct [[nodiscard]] Vector2 {
+	static const int AXIS_COUNT = 2;
+
+	enum Axis {
+		AXIS_X,
+		AXIS_Y,
+	};
+
+	union {
+		// NOLINTBEGIN(modernize-use-default-member-init)
+		struct {
+			real_t x;
+			real_t y;
+		};
+
+		struct {
+			real_t width;
+			real_t height;
+		};
+
+		real_t coord[2] = { 0 };
+		// NOLINTEND(modernize-use-default-member-init)
+	};
+
+	_FORCE_INLINE_ real_t &operator[](int p_axis) {
+		DEV_ASSERT((unsigned int)p_axis < 2);
+		return coord[p_axis];
+	}
+	_FORCE_INLINE_ const real_t &operator[](int p_axis) const {
+		DEV_ASSERT((unsigned int)p_axis < 2);
+		return coord[p_axis];
+	}
+
+	_FORCE_INLINE_ Vector2::Axis min_axis_index() const {
+		return x < y ? Vector2::AXIS_X : Vector2::AXIS_Y;
+	}
+
+	_FORCE_INLINE_ Vector2::Axis max_axis_index() const {
+		return x < y ? Vector2::AXIS_Y : Vector2::AXIS_X;
+	}
+
+	void normalize();
+	Vector2 normalized() const;
+	bool is_normalized() const;
+
+	real_t length() const;
+	real_t length_squared() const;
+	Vector2 limit_length(real_t p_len = 1.0) const;
+
+	Vector2 min(const Vector2 &p_vector2) const {
+		return Vector2(MIN(x, p_vector2.x), MIN(y, p_vector2.y));
+	}
+
+	Vector2 minf(real_t p_scalar) const {
+		return Vector2(MIN(x, p_scalar), MIN(y, p_scalar));
+	}
+
+	Vector2 max(const Vector2 &p_vector2) const {
+		return Vector2(MAX(x, p_vector2.x), MAX(y, p_vector2.y));
+	}
+
+	Vector2 maxf(real_t p_scalar) const {
+		return Vector2(MAX(x, p_scalar), MAX(y, p_scalar));
+	}
+
+	real_t distance_to(const Vector2 &p_vector2) const;
+	real_t distance_squared_to(const Vector2 &p_vector2) const;
+	real_t angle_to(const Vector2 &p_vector2) const;
+	real_t angle_to_point(const Vector2 &p_vector2) const;
+	_FORCE_INLINE_ Vector2 direction_to(const Vector2 &p_to) const;
+
+	real_t dot(const Vector2 &p_other) const;
+	real_t cross(const Vector2 &p_other) const;
+	Vector2 posmod(real_t p_mod) const;
+	Vector2 posmodv(const Vector2 &p_modv) const;
+	Vector2 project(const Vector2 &p_to) const;
+
+	Vector2 plane_project(real_t p_d, const Vector2 &p_vec) const;
+
+	_FORCE_INLINE_ Vector2 lerp(const Vector2 &p_to, real_t p_weight) const;
+	_FORCE_INLINE_ Vector2 slerp(const Vector2 &p_to, real_t p_weight) const;
+	_FORCE_INLINE_ Vector2 cubic_interpolate(const Vector2 &p_b, const Vector2 &p_pre_a, const Vector2 &p_post_b, real_t p_weight) const;
+	_FORCE_INLINE_ Vector2 cubic_interpolate_in_time(const Vector2 &p_b, const Vector2 &p_pre_a, const Vector2 &p_post_b, real_t p_weight, real_t p_b_t, real_t p_pre_a_t, real_t p_post_b_t) const;
+	_FORCE_INLINE_ Vector2 bezier_interpolate(const Vector2 &p_control_1, const Vector2 &p_control_2, const Vector2 &p_end, real_t p_t) const;
+	_FORCE_INLINE_ Vector2 bezier_derivative(const Vector2 &p_control_1, const Vector2 &p_control_2, const Vector2 &p_end, real_t p_t) const;
+
+	Vector2 move_toward(const Vector2 &p_to, real_t p_delta) const;
+
+	Vector2 slide(const Vector2 &p_normal) const;
+	Vector2 bounce(const Vector2 &p_normal) const;
+	Vector2 reflect(const Vector2 &p_normal) const;
+
+	bool is_equal_approx(const Vector2 &p_v) const;
+	bool is_same(const Vector2 &p_v) const;
+	bool is_zero_approx() const;
+	bool is_finite() const;
+
+	constexpr Vector2 operator+(const Vector2 &p_v) const;
+	constexpr void operator+=(const Vector2 &p_v);
+	constexpr Vector2 operator-(const Vector2 &p_v) const;
+	constexpr void operator-=(const Vector2 &p_v);
+	constexpr Vector2 operator*(const Vector2 &p_v1) const;
+
+	constexpr Vector2 operator*(real_t p_rvalue) const;
+	constexpr void operator*=(real_t p_rvalue);
+	constexpr void operator*=(const Vector2 &p_rvalue) { *this = *this * p_rvalue; }
+
+	constexpr Vector2 operator/(const Vector2 &p_v1) const;
+
+	constexpr Vector2 operator/(real_t p_rvalue) const;
+
+	constexpr void operator/=(real_t p_rvalue);
+	constexpr void operator/=(const Vector2 &p_rvalue) { *this = *this / p_rvalue; }
+
+	constexpr Vector2 operator-() const;
+
+	constexpr bool operator==(const Vector2 &p_vec2) const;
+	constexpr bool operator!=(const Vector2 &p_vec2) const;
+
+	constexpr bool operator<(const Vector2 &p_vec2) const { return x == p_vec2.x ? (y < p_vec2.y) : (x < p_vec2.x); }
+	constexpr bool operator>(const Vector2 &p_vec2) const { return x == p_vec2.x ? (y > p_vec2.y) : (x > p_vec2.x); }
+	constexpr bool operator<=(const Vector2 &p_vec2) const { return x == p_vec2.x ? (y <= p_vec2.y) : (x < p_vec2.x); }
+	constexpr bool operator>=(const Vector2 &p_vec2) const { return x == p_vec2.x ? (y >= p_vec2.y) : (x > p_vec2.x); }
+
+	real_t angle() const;
+	static Vector2 from_angle(real_t p_angle);
+
+	_FORCE_INLINE_ Vector2 abs() const {
+		return Vector2(Math::abs(x), Math::abs(y));
+	}
+
+	Vector2 rotated(real_t p_by) const;
+	Vector2 orthogonal() const {
+		return Vector2(y, -x);
+	}
+
+	Vector2 sign() const;
+	Vector2 floor() const;
+	Vector2 ceil() const;
+	Vector2 round() const;
+	Vector2 snapped(const Vector2 &p_by) const;
+	Vector2 snappedf(real_t p_by) const;
+	Vector2 clamp(const Vector2 &p_min, const Vector2 &p_max) const;
+	Vector2 clampf(real_t p_min, real_t p_max) const;
+	real_t aspect() const { return width / height; }
+
+	explicit operator String() const;
+	operator Vector2i() const;
+
+	// NOLINTBEGIN(cppcoreguidelines-pro-type-member-init)
+	constexpr Vector2() :
+			x(0), y(0) {}
+	constexpr Vector2(real_t p_x, real_t p_y) :
+			x(p_x), y(p_y) {}
+	// NOLINTEND(cppcoreguidelines-pro-type-member-init)
+};
+
+_FORCE_INLINE_ Vector2 Vector2::plane_project(real_t p_d, const Vector2 &p_vec) const {
+	return p_vec - *this * (dot(p_vec) - p_d);
+}
+
+constexpr Vector2 Vector2::operator+(const Vector2 &p_v) const {
+	return Vector2(x + p_v.x, y + p_v.y);
+}
+
+constexpr void Vector2::operator+=(const Vector2 &p_v) {
+	x += p_v.x;
+	y += p_v.y;
+}
+
+constexpr Vector2 Vector2::operator-(const Vector2 &p_v) const {
+	return Vector2(x - p_v.x, y - p_v.y);
+}
+
+constexpr void Vector2::operator-=(const Vector2 &p_v) {
+	x -= p_v.x;
+	y -= p_v.y;
+}
+
+constexpr Vector2 Vector2::operator*(const Vector2 &p_v1) const {
+	return Vector2(x * p_v1.x, y * p_v1.y);
+}
+
+constexpr Vector2 Vector2::operator*(real_t p_rvalue) const {
+	return Vector2(x * p_rvalue, y * p_rvalue);
+}
+
+constexpr void Vector2::operator*=(real_t p_rvalue) {
+	x *= p_rvalue;
+	y *= p_rvalue;
+}
+
+constexpr Vector2 Vector2::operator/(const Vector2 &p_v1) const {
+	return Vector2(x / p_v1.x, y / p_v1.y);
+}
+
+constexpr Vector2 Vector2::operator/(real_t p_rvalue) const {
+	return Vector2(x / p_rvalue, y / p_rvalue);
+}
+
+constexpr void Vector2::operator/=(real_t p_rvalue) {
+	x /= p_rvalue;
+	y /= p_rvalue;
+}
+
+constexpr Vector2 Vector2::operator-() const {
+	return Vector2(-x, -y);
+}
+
+constexpr bool Vector2::operator==(const Vector2 &p_vec2) const {
+	return x == p_vec2.x && y == p_vec2.y;
+}
+
+constexpr bool Vector2::operator!=(const Vector2 &p_vec2) const {
+	return x != p_vec2.x || y != p_vec2.y;
+}
+
+Vector2 Vector2::lerp(const Vector2 &p_to, real_t p_weight) const {
+	Vector2 res = *this;
+	res.x = Math::lerp(res.x, p_to.x, p_weight);
+	res.y = Math::lerp(res.y, p_to.y, p_weight);
+	return res;
+}
+
+Vector2 Vector2::slerp(const Vector2 &p_to, real_t p_weight) const {
+	real_t start_length_sq = length_squared();
+	real_t end_length_sq = p_to.length_squared();
+	if (unlikely(start_length_sq == 0.0f || end_length_sq == 0.0f)) {
+		// Zero length vectors have no angle, so the best we can do is either lerp or throw an error.
+		return lerp(p_to, p_weight);
+	}
+	real_t start_length = Math::sqrt(start_length_sq);
+	real_t result_length = Math::lerp(start_length, Math::sqrt(end_length_sq), p_weight);
+	real_t angle = angle_to(p_to);
+	return rotated(angle * p_weight) * (result_length / start_length);
+}
+
+Vector2 Vector2::cubic_interpolate(const Vector2 &p_b, const Vector2 &p_pre_a, const Vector2 &p_post_b, real_t p_weight) const {
+	Vector2 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);
+	return res;
+}
+
+Vector2 Vector2::cubic_interpolate_in_time(const Vector2 &p_b, const Vector2 &p_pre_a, const Vector2 &p_post_b, real_t p_weight, real_t p_b_t, real_t p_pre_a_t, real_t p_post_b_t) const {
+	Vector2 res = *this;
+	res.x = Math::cubic_interpolate_in_time(res.x, p_b.x, p_pre_a.x, p_post_b.x, p_weight, p_b_t, p_pre_a_t, p_post_b_t);
+	res.y = Math::cubic_interpolate_in_time(res.y, p_b.y, p_pre_a.y, p_post_b.y, p_weight, p_b_t, p_pre_a_t, p_post_b_t);
+	return res;
+}
+
+Vector2 Vector2::bezier_interpolate(const Vector2 &p_control_1, const Vector2 &p_control_2, const Vector2 &p_end, real_t p_t) const {
+	Vector2 res = *this;
+	res.x = Math::bezier_interpolate(res.x, p_control_1.x, p_control_2.x, p_end.x, p_t);
+	res.y = Math::bezier_interpolate(res.y, p_control_1.y, p_control_2.y, p_end.y, p_t);
+	return res;
+}
+
+Vector2 Vector2::bezier_derivative(const Vector2 &p_control_1, const Vector2 &p_control_2, const Vector2 &p_end, real_t p_t) const {
+	Vector2 res = *this;
+	res.x = Math::bezier_derivative(res.x, p_control_1.x, p_control_2.x, p_end.x, p_t);
+	res.y = Math::bezier_derivative(res.y, p_control_1.y, p_control_2.y, p_end.y, p_t);
+	return res;
+}
+
+Vector2 Vector2::direction_to(const Vector2 &p_to) const {
+	Vector2 ret(p_to.x - x, p_to.y - y);
+	ret.normalize();
+	return ret;
+}
+
+// Multiplication operators required to workaround issues with LLVM using implicit conversion
+// to Vector2i instead for integers where it should not.
+
+constexpr Vector2 operator*(float p_scalar, const Vector2 &p_vec) {
+	return p_vec * p_scalar;
+}
+
+constexpr Vector2 operator*(double p_scalar, const Vector2 &p_vec) {
+	return p_vec * p_scalar;
+}
+
+constexpr Vector2 operator*(int32_t p_scalar, const Vector2 &p_vec) {
+	return p_vec * p_scalar;
+}
+
+constexpr Vector2 operator*(int64_t p_scalar, const Vector2 &p_vec) {
+	return p_vec * p_scalar;
+}
+
+typedef Vector2 Size2;
+typedef Vector2 Point2;
+
+template <>
+struct is_zero_constructible<Vector2> : std::true_type {};

core/math/vector2i.cpp

@@ -0,0 +1,74 @@
+/**************************************************************************/
+/*  vector2i.cpp                                                          */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "vector2i.h"
+
+#include "core/math/vector2.h"
+#include "core/string/ustring.h"
+
+Vector2i Vector2i::clamp(const Vector2i &p_min, const Vector2i &p_max) const {
+	return Vector2i(
+			CLAMP(x, p_min.x, p_max.x),
+			CLAMP(y, p_min.y, p_max.y));
+}
+
+Vector2i Vector2i::clampi(int32_t p_min, int32_t p_max) const {
+	return Vector2i(
+			CLAMP(x, p_min, p_max),
+			CLAMP(y, p_min, p_max));
+}
+
+Vector2i Vector2i::snapped(const Vector2i &p_step) const {
+	return Vector2i(
+			Math::snapped(x, p_step.x),
+			Math::snapped(y, p_step.y));
+}
+
+Vector2i Vector2i::snappedi(int32_t p_step) const {
+	return Vector2i(
+			Math::snapped(x, p_step),
+			Math::snapped(y, p_step));
+}
+
+int64_t Vector2i::length_squared() const {
+	return x * (int64_t)x + y * (int64_t)y;
+}
+
+double Vector2i::length() const {
+	return Math::sqrt((double)length_squared());
+}
+
+Vector2i::operator String() const {
+	return "(" + itos(x) + ", " + itos(y) + ")";
+}
+
+Vector2i::operator Vector2() const {
+	return Vector2((int32_t)x, (int32_t)y);
+}

core/math/vector2i.h

@@ -0,0 +1,244 @@
+/**************************************************************************/
+/*  vector2i.h                                                            */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "core/error/error_macros.h"
+#include "core/math/math_funcs.h"
+
+class String;
+struct Vector2;
+
+struct [[nodiscard]] Vector2i {
+	static const int AXIS_COUNT = 2;
+
+	enum Axis {
+		AXIS_X,
+		AXIS_Y,
+	};
+
+	union {
+		// NOLINTBEGIN(modernize-use-default-member-init)
+		struct {
+			int32_t x;
+			int32_t y;
+		};
+
+		struct {
+			int32_t width;
+			int32_t height;
+		};
+
+		int32_t coord[2] = { 0 };
+		// NOLINTEND(modernize-use-default-member-init)
+	};
+
+	_FORCE_INLINE_ int32_t &operator[](int p_axis) {
+		DEV_ASSERT((unsigned int)p_axis < 2);
+		return coord[p_axis];
+	}
+	_FORCE_INLINE_ const int32_t &operator[](int p_axis) const {
+		DEV_ASSERT((unsigned int)p_axis < 2);
+		return coord[p_axis];
+	}
+
+	_FORCE_INLINE_ Vector2i::Axis min_axis_index() const {
+		return x < y ? Vector2i::AXIS_X : Vector2i::AXIS_Y;
+	}
+
+	_FORCE_INLINE_ Vector2i::Axis max_axis_index() const {
+		return x < y ? Vector2i::AXIS_Y : Vector2i::AXIS_X;
+	}
+
+	Vector2i min(const Vector2i &p_vector2i) const {
+		return Vector2i(MIN(x, p_vector2i.x), MIN(y, p_vector2i.y));
+	}
+
+	Vector2i mini(int32_t p_scalar) const {
+		return Vector2i(MIN(x, p_scalar), MIN(y, p_scalar));
+	}
+
+	Vector2i max(const Vector2i &p_vector2i) const {
+		return Vector2i(MAX(x, p_vector2i.x), MAX(y, p_vector2i.y));
+	}
+
+	Vector2i maxi(int32_t p_scalar) const {
+		return Vector2i(MAX(x, p_scalar), MAX(y, p_scalar));
+	}
+
+	double distance_to(const Vector2i &p_to) const {
+		return (p_to - *this).length();
+	}
+
+	int64_t distance_squared_to(const Vector2i &p_to) const {
+		return (p_to - *this).length_squared();
+	}
+
+	constexpr Vector2i operator+(const Vector2i &p_v) const;
+	constexpr void operator+=(const Vector2i &p_v);
+	constexpr Vector2i operator-(const Vector2i &p_v) const;
+	constexpr void operator-=(const Vector2i &p_v);
+	constexpr Vector2i operator*(const Vector2i &p_v1) const;
+
+	constexpr Vector2i operator*(int32_t p_rvalue) const;
+	constexpr void operator*=(int32_t p_rvalue);
+
+	constexpr Vector2i operator/(const Vector2i &p_v1) const;
+	constexpr Vector2i operator/(int32_t p_rvalue) const;
+	constexpr void operator/=(int32_t p_rvalue);
+
+	constexpr Vector2i operator%(const Vector2i &p_v1) const;
+	constexpr Vector2i operator%(int32_t p_rvalue) const;
+	constexpr void operator%=(int32_t p_rvalue);
+
+	constexpr Vector2i operator-() const;
+	constexpr bool operator<(const Vector2i &p_vec2) const { return (x == p_vec2.x) ? (y < p_vec2.y) : (x < p_vec2.x); }
+	constexpr bool operator>(const Vector2i &p_vec2) const { return (x == p_vec2.x) ? (y > p_vec2.y) : (x > p_vec2.x); }
+
+	constexpr bool operator<=(const Vector2i &p_vec2) const { return x == p_vec2.x ? (y <= p_vec2.y) : (x < p_vec2.x); }
+	constexpr bool operator>=(const Vector2i &p_vec2) const { return x == p_vec2.x ? (y >= p_vec2.y) : (x > p_vec2.x); }
+
+	constexpr bool operator==(const Vector2i &p_vec2) const;
+	constexpr bool operator!=(const Vector2i &p_vec2) const;
+
+	int64_t length_squared() const;
+	double length() const;
+
+	real_t aspect() const { return width / (real_t)height; }
+	Vector2i sign() const { return Vector2i(SIGN(x), SIGN(y)); }
+	Vector2i abs() const { return Vector2i(Math::abs(x), Math::abs(y)); }
+	Vector2i clamp(const Vector2i &p_min, const Vector2i &p_max) const;
+	Vector2i clampi(int32_t p_min, int32_t p_max) const;
+	Vector2i snapped(const Vector2i &p_step) const;
+	Vector2i snappedi(int32_t p_step) const;
+
+	explicit operator String() const;
+	operator Vector2() const;
+
+	// NOLINTBEGIN(cppcoreguidelines-pro-type-member-init)
+	constexpr Vector2i() :
+			x(0), y(0) {}
+	constexpr Vector2i(int32_t p_x, int32_t p_y) :
+			x(p_x), y(p_y) {}
+	// NOLINTEND(cppcoreguidelines-pro-type-member-init)
+};
+
+constexpr Vector2i Vector2i::operator+(const Vector2i &p_v) const {
+	return Vector2i(x + p_v.x, y + p_v.y);
+}
+
+constexpr void Vector2i::operator+=(const Vector2i &p_v) {
+	x += p_v.x;
+	y += p_v.y;
+}
+
+constexpr Vector2i Vector2i::operator-(const Vector2i &p_v) const {
+	return Vector2i(x - p_v.x, y - p_v.y);
+}
+
+constexpr void Vector2i::operator-=(const Vector2i &p_v) {
+	x -= p_v.x;
+	y -= p_v.y;
+}
+
+constexpr Vector2i Vector2i::operator*(const Vector2i &p_v1) const {
+	return Vector2i(x * p_v1.x, y * p_v1.y);
+}
+
+constexpr Vector2i Vector2i::operator*(int32_t p_rvalue) const {
+	return Vector2i(x * p_rvalue, y * p_rvalue);
+}
+
+constexpr void Vector2i::operator*=(int32_t p_rvalue) {
+	x *= p_rvalue;
+	y *= p_rvalue;
+}
+
+constexpr Vector2i Vector2i::operator/(const Vector2i &p_v1) const {
+	return Vector2i(x / p_v1.x, y / p_v1.y);
+}
+
+constexpr Vector2i Vector2i::operator/(int32_t p_rvalue) const {
+	return Vector2i(x / p_rvalue, y / p_rvalue);
+}
+
+constexpr void Vector2i::operator/=(int32_t p_rvalue) {
+	x /= p_rvalue;
+	y /= p_rvalue;
+}
+
+constexpr Vector2i Vector2i::operator%(const Vector2i &p_v1) const {
+	return Vector2i(x % p_v1.x, y % p_v1.y);
+}
+
+constexpr Vector2i Vector2i::operator%(int32_t p_rvalue) const {
+	return Vector2i(x % p_rvalue, y % p_rvalue);
+}
+
+constexpr void Vector2i::operator%=(int32_t p_rvalue) {
+	x %= p_rvalue;
+	y %= p_rvalue;
+}
+
+constexpr Vector2i Vector2i::operator-() const {
+	return Vector2i(-x, -y);
+}
+
+constexpr bool Vector2i::operator==(const Vector2i &p_vec2) const {
+	return x == p_vec2.x && y == p_vec2.y;
+}
+
+constexpr bool Vector2i::operator!=(const Vector2i &p_vec2) const {
+	return x != p_vec2.x || y != p_vec2.y;
+}
+
+// Multiplication operators required to workaround issues with LLVM using implicit conversion.
+
+constexpr Vector2i operator*(int32_t p_scalar, const Vector2i &p_vector) {
+	return p_vector * p_scalar;
+}
+
+constexpr Vector2i operator*(int64_t p_scalar, const Vector2i &p_vector) {
+	return p_vector * p_scalar;
+}
+
+constexpr Vector2i operator*(float p_scalar, const Vector2i &p_vector) {
+	return p_vector * p_scalar;
+}
+
+constexpr Vector2i operator*(double p_scalar, const Vector2i &p_vector) {
+	return p_vector * p_scalar;
+}
+
+typedef Vector2i Size2i;
+typedef Vector2i Point2i;
+
+template <>
+struct is_zero_constructible<Vector2i> : std::true_type {};

core/math/vector3.cpp

@@ -0,0 +1,177 @@
+/**************************************************************************/
+/*  vector3.cpp                                                           */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#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, real_t p_angle) {
+	*this = Basis(p_axis, p_angle).xform(*this);
+}
+
+Vector3 Vector3::rotated(const Vector3 &p_axis, real_t p_angle) const {
+	Vector3 r = *this;
+	r.rotate(p_axis, p_angle);
+	return r;
+}
+
+Vector3 Vector3::clamp(const Vector3 &p_min, const Vector3 &p_max) const {
+	return Vector3(
+			CLAMP(x, p_min.x, p_max.x),
+			CLAMP(y, p_min.y, p_max.y),
+			CLAMP(z, p_min.z, p_max.z));
+}
+
+Vector3 Vector3::clampf(real_t p_min, real_t p_max) const {
+	return Vector3(
+			CLAMP(x, p_min, p_max),
+			CLAMP(y, p_min, p_max),
+			CLAMP(z, p_min, p_max));
+}
+
+void Vector3::snap(const Vector3 &p_step) {
+	x = Math::snapped(x, p_step.x);
+	y = Math::snapped(y, p_step.y);
+	z = Math::snapped(z, p_step.z);
+}
+
+Vector3 Vector3::snapped(const Vector3 &p_step) const {
+	Vector3 v = *this;
+	v.snap(p_step);
+	return v;
+}
+
+void Vector3::snapf(real_t p_step) {
+	x = Math::snapped(x, p_step);
+	y = Math::snapped(y, p_step);
+	z = Math::snapped(z, p_step);
+}
+
+Vector3 Vector3::snappedf(real_t p_step) const {
+	Vector3 v = *this;
+	v.snapf(p_step);
+	return v;
+}
+
+Vector3 Vector3::limit_length(real_t p_len) const {
+	const real_t l = length();
+	Vector3 v = *this;
+	if (l > 0 && p_len < l) {
+		v /= l;
+		v *= p_len;
+	}
+
+	return v;
+}
+
+Vector3 Vector3::move_toward(const Vector3 &p_to, real_t p_delta) const {
+	Vector3 v = *this;
+	Vector3 vd = p_to - v;
+	real_t len = vd.length();
+	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));
+	const real_t 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();
+}
+
+Vector2 Vector3::octahedron_tangent_encode(float p_sign) const {
+	const real_t bias = 1.0f / (real_t)32767.0f;
+	Vector2 res = octahedron_encode();
+	res.y = MAX(res.y, bias);
+	res.y = res.y * 0.5f + 0.5f;
+	res.y = p_sign >= 0.0f ? res.y : 1 - res.y;
+	return res;
+}
+
+Vector3 Vector3::octahedron_tangent_decode(const Vector2 &p_oct, float *r_sign) {
+	Vector2 oct_compressed = p_oct;
+	oct_compressed.y = oct_compressed.y * 2 - 1;
+	*r_sign = oct_compressed.y >= 0.0f ? 1.0f : -1.0f;
+	oct_compressed.y = Math::abs(oct_compressed.y);
+	Vector3 res = Vector3::octahedron_decode(oct_compressed);
+	return res;
+}
+
+Basis Vector3::outer(const Vector3 &p_with) const {
+	Basis basis;
+	basis.rows[0] = Vector3(x * p_with.x, x * p_with.y, x * p_with.z);
+	basis.rows[1] = Vector3(y * p_with.x, y * p_with.y, y * p_with.z);
+	basis.rows[2] = Vector3(z * p_with.x, z * p_with.y, z * p_with.z);
+	return basis;
+}
+
+bool Vector3::is_equal_approx(const Vector3 &p_v) const {
+	return Math::is_equal_approx(x, p_v.x) && Math::is_equal_approx(y, p_v.y) && Math::is_equal_approx(z, p_v.z);
+}
+
+bool Vector3::is_same(const Vector3 &p_v) const {
+	return Math::is_same(x, p_v.x) && Math::is_same(y, p_v.y) && Math::is_same(z, p_v.z);
+}
+
+bool Vector3::is_zero_approx() const {
+	return Math::is_zero_approx(x) && Math::is_zero_approx(y) && Math::is_zero_approx(z);
+}
+
+bool Vector3::is_finite() const {
+	return Math::is_finite(x) && Math::is_finite(y) && Math::is_finite(z);
+}
+
+Vector3::operator String() const {
+	return "(" + String::num_real(x, true) + ", " + String::num_real(y, true) + ", " + String::num_real(z, true) + ")";
+}
+
+Vector3::operator Vector3i() const {
+	return Vector3i(x, y, z);
+}

core/math/vector3.h

@@ -0,0 +1,554 @@
+/**************************************************************************/
+/*  vector3.h                                                             */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "core/error/error_macros.h"
+#include "core/math/math_funcs.h"
+#include "core/string/ustring.h"
+
+struct Basis;
+struct Vector2;
+struct Vector3i;
+
+struct [[nodiscard]] Vector3 {
+	static const int AXIS_COUNT = 3;
+
+	enum Axis {
+		AXIS_X,
+		AXIS_Y,
+		AXIS_Z,
+	};
+
+	union {
+		// NOLINTBEGIN(modernize-use-default-member-init)
+		struct {
+			real_t x;
+			real_t y;
+			real_t z;
+		};
+
+		real_t coord[3] = { 0 };
+		// NOLINTEND(modernize-use-default-member-init)
+	};
+
+	_FORCE_INLINE_ const real_t &operator[](int p_axis) const {
+		DEV_ASSERT((unsigned int)p_axis < 3);
+		return coord[p_axis];
+	}
+
+	_FORCE_INLINE_ real_t &operator[](int p_axis) {
+		DEV_ASSERT((unsigned int)p_axis < 3);
+		return coord[p_axis];
+	}
+
+	_FORCE_INLINE_ Vector3::Axis min_axis_index() const {
+		return x < y ? (x < z ? Vector3::AXIS_X : Vector3::AXIS_Z) : (y < z ? Vector3::AXIS_Y : Vector3::AXIS_Z);
+	}
+
+	_FORCE_INLINE_ Vector3::Axis max_axis_index() const {
+		return x < y ? (y < z ? Vector3::AXIS_Z : Vector3::AXIS_Y) : (x < z ? Vector3::AXIS_Z : Vector3::AXIS_X);
+	}
+
+	Vector3 min(const Vector3 &p_vector3) const {
+		return Vector3(MIN(x, p_vector3.x), MIN(y, p_vector3.y), MIN(z, p_vector3.z));
+	}
+
+	Vector3 minf(real_t p_scalar) const {
+		return Vector3(MIN(x, p_scalar), MIN(y, p_scalar), MIN(z, p_scalar));
+	}
+
+	Vector3 max(const Vector3 &p_vector3) const {
+		return Vector3(MAX(x, p_vector3.x), MAX(y, p_vector3.y), MAX(z, p_vector3.z));
+	}
+
+	Vector3 maxf(real_t p_scalar) const {
+		return Vector3(MAX(x, p_scalar), MAX(y, p_scalar), MAX(z, p_scalar));
+	}
+
+	_FORCE_INLINE_ real_t length() const;
+	_FORCE_INLINE_ real_t length_squared() const;
+
+	_FORCE_INLINE_ void normalize();
+	_FORCE_INLINE_ Vector3 normalized() const;
+	_FORCE_INLINE_ bool is_normalized() const;
+	_FORCE_INLINE_ Vector3 inverse() const;
+	Vector3 limit_length(real_t p_len = 1.0) const;
+
+	_FORCE_INLINE_ void zero();
+
+	void snap(const Vector3 &p_step);
+	void snapf(real_t p_step);
+	Vector3 snapped(const Vector3 &p_step) const;
+	Vector3 snappedf(real_t p_step) const;
+
+	void rotate(const Vector3 &p_axis, real_t p_angle);
+	Vector3 rotated(const Vector3 &p_axis, real_t p_angle) const;
+
+	/* Static Methods between 2 vector3s */
+
+	_FORCE_INLINE_ Vector3 lerp(const Vector3 &p_to, real_t p_weight) const;
+	_FORCE_INLINE_ Vector3 slerp(const Vector3 &p_to, real_t p_weight) const;
+	_FORCE_INLINE_ Vector3 cubic_interpolate(const Vector3 &p_b, const Vector3 &p_pre_a, const Vector3 &p_post_b, real_t p_weight) const;
+	_FORCE_INLINE_ Vector3 cubic_interpolate_in_time(const Vector3 &p_b, const Vector3 &p_pre_a, const Vector3 &p_post_b, real_t p_weight, real_t p_b_t, real_t p_pre_a_t, real_t p_post_b_t) const;
+	_FORCE_INLINE_ Vector3 bezier_interpolate(const Vector3 &p_control_1, const Vector3 &p_control_2, const Vector3 &p_end, real_t p_t) const;
+	_FORCE_INLINE_ Vector3 bezier_derivative(const Vector3 &p_control_1, const Vector3 &p_control_2, const Vector3 &p_end, real_t p_t) const;
+
+	Vector3 move_toward(const Vector3 &p_to, real_t p_delta) const;
+
+	Vector2 octahedron_encode() const;
+	static Vector3 octahedron_decode(const Vector2 &p_oct);
+	Vector2 octahedron_tangent_encode(float p_sign) const;
+	static Vector3 octahedron_tangent_decode(const Vector2 &p_oct, float *r_sign);
+
+	_FORCE_INLINE_ Vector3 cross(const Vector3 &p_with) const;
+	_FORCE_INLINE_ real_t dot(const Vector3 &p_with) const;
+	Basis outer(const Vector3 &p_with) const;
+	_FORCE_INLINE_ Vector3 get_any_perpendicular() const;
+
+	_FORCE_INLINE_ Vector3 abs() const;
+	_FORCE_INLINE_ Vector3 floor() const;
+	_FORCE_INLINE_ Vector3 sign() const;
+	_FORCE_INLINE_ Vector3 ceil() const;
+	_FORCE_INLINE_ Vector3 round() const;
+	Vector3 clamp(const Vector3 &p_min, const Vector3 &p_max) const;
+	Vector3 clampf(real_t p_min, real_t p_max) const;
+
+	_FORCE_INLINE_ real_t distance_to(const Vector3 &p_to) const;
+	_FORCE_INLINE_ real_t distance_squared_to(const Vector3 &p_to) const;
+
+	_FORCE_INLINE_ Vector3 posmod(real_t p_mod) const;
+	_FORCE_INLINE_ Vector3 posmodv(const Vector3 &p_modv) const;
+	_FORCE_INLINE_ Vector3 project(const Vector3 &p_to) const;
+
+	_FORCE_INLINE_ real_t angle_to(const Vector3 &p_to) const;
+	_FORCE_INLINE_ real_t signed_angle_to(const Vector3 &p_to, const Vector3 &p_axis) const;
+	_FORCE_INLINE_ Vector3 direction_to(const Vector3 &p_to) const;
+
+	_FORCE_INLINE_ Vector3 slide(const Vector3 &p_normal) const;
+	_FORCE_INLINE_ Vector3 bounce(const Vector3 &p_normal) const;
+	_FORCE_INLINE_ Vector3 reflect(const Vector3 &p_normal) const;
+
+	bool is_equal_approx(const Vector3 &p_v) const;
+	bool is_same(const Vector3 &p_v) const;
+	bool is_zero_approx() const;
+	bool is_finite() const;
+
+	/* Operators */
+
+	constexpr Vector3 &operator+=(const Vector3 &p_v);
+	constexpr Vector3 operator+(const Vector3 &p_v) const;
+	constexpr Vector3 &operator-=(const Vector3 &p_v);
+	constexpr Vector3 operator-(const Vector3 &p_v) const;
+	constexpr Vector3 &operator*=(const Vector3 &p_v);
+	constexpr Vector3 operator*(const Vector3 &p_v) const;
+	constexpr Vector3 &operator/=(const Vector3 &p_v);
+	constexpr Vector3 operator/(const Vector3 &p_v) const;
+
+	constexpr Vector3 &operator*=(real_t p_scalar);
+	constexpr Vector3 operator*(real_t p_scalar) const;
+	constexpr Vector3 &operator/=(real_t p_scalar);
+	constexpr Vector3 operator/(real_t p_scalar) const;
+
+	constexpr Vector3 operator-() const;
+
+	constexpr bool operator==(const Vector3 &p_v) const;
+	constexpr bool operator!=(const Vector3 &p_v) const;
+	constexpr bool operator<(const Vector3 &p_v) const;
+	constexpr bool operator<=(const Vector3 &p_v) const;
+	constexpr bool operator>(const Vector3 &p_v) const;
+	constexpr bool operator>=(const Vector3 &p_v) const;
+
+	explicit operator String() const;
+	operator Vector3i() const;
+
+	constexpr Vector3() :
+			x(0), y(0), z(0) {}
+	constexpr Vector3(real_t p_x, real_t p_y, real_t p_z) :
+			x(p_x), y(p_y), z(p_z) {}
+};
+
+Vector3 Vector3::cross(const Vector3 &p_with) const {
+	Vector3 ret(
+			(y * p_with.z) - (z * p_with.y),
+			(z * p_with.x) - (x * p_with.z),
+			(x * p_with.y) - (y * p_with.x));
+
+	return ret;
+}
+
+real_t Vector3::dot(const Vector3 &p_with) const {
+	return x * p_with.x + y * p_with.y + z * p_with.z;
+}
+
+Vector3 Vector3::abs() const {
+	return Vector3(Math::abs(x), Math::abs(y), Math::abs(z));
+}
+
+Vector3 Vector3::sign() const {
+	return Vector3(SIGN(x), SIGN(y), SIGN(z));
+}
+
+Vector3 Vector3::floor() const {
+	return Vector3(Math::floor(x), Math::floor(y), Math::floor(z));
+}
+
+Vector3 Vector3::ceil() const {
+	return Vector3(Math::ceil(x), Math::ceil(y), Math::ceil(z));
+}
+
+Vector3 Vector3::round() const {
+	return Vector3(Math::round(x), Math::round(y), Math::round(z));
+}
+
+Vector3 Vector3::lerp(const Vector3 &p_to, real_t p_weight) const {
+	Vector3 res = *this;
+	res.x = Math::lerp(res.x, p_to.x, p_weight);
+	res.y = Math::lerp(res.y, p_to.y, p_weight);
+	res.z = Math::lerp(res.z, p_to.z, p_weight);
+	return res;
+}
+
+Vector3 Vector3::slerp(const Vector3 &p_to, real_t p_weight) const {
+	// This method seems more complicated than it really is, since we write out
+	// the internals of some methods for efficiency (mainly, checking length).
+	real_t start_length_sq = length_squared();
+	real_t end_length_sq = p_to.length_squared();
+	if (unlikely(start_length_sq == 0.0f || end_length_sq == 0.0f)) {
+		// Zero length vectors have no angle, so the best we can do is either lerp or throw an error.
+		return lerp(p_to, p_weight);
+	}
+	Vector3 axis = cross(p_to);
+	real_t axis_length_sq = axis.length_squared();
+	if (unlikely(axis_length_sq == 0.0f)) {
+		// Colinear vectors have no rotation axis or angle between them, so the best we can do is lerp.
+		return lerp(p_to, p_weight);
+	}
+	axis /= Math::sqrt(axis_length_sq);
+	real_t start_length = Math::sqrt(start_length_sq);
+	real_t result_length = Math::lerp(start_length, Math::sqrt(end_length_sq), p_weight);
+	real_t angle = angle_to(p_to);
+	return rotated(axis, angle * p_weight) * (result_length / start_length);
+}
+
+Vector3 Vector3::cubic_interpolate(const Vector3 &p_b, const Vector3 &p_pre_a, const Vector3 &p_post_b, real_t p_weight) const {
+	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::cubic_interpolate_in_time(const Vector3 &p_b, const Vector3 &p_pre_a, const Vector3 &p_post_b, real_t p_weight, real_t p_b_t, real_t p_pre_a_t, real_t p_post_b_t) const {
+	Vector3 res = *this;
+	res.x = Math::cubic_interpolate_in_time(res.x, p_b.x, p_pre_a.x, p_post_b.x, p_weight, p_b_t, p_pre_a_t, p_post_b_t);
+	res.y = Math::cubic_interpolate_in_time(res.y, p_b.y, p_pre_a.y, p_post_b.y, p_weight, p_b_t, p_pre_a_t, p_post_b_t);
+	res.z = Math::cubic_interpolate_in_time(res.z, p_b.z, p_pre_a.z, p_post_b.z, p_weight, p_b_t, p_pre_a_t, p_post_b_t);
+	return res;
+}
+
+Vector3 Vector3::bezier_interpolate(const Vector3 &p_control_1, const Vector3 &p_control_2, const Vector3 &p_end, real_t p_t) const {
+	Vector3 res = *this;
+	res.x = Math::bezier_interpolate(res.x, p_control_1.x, p_control_2.x, p_end.x, p_t);
+	res.y = Math::bezier_interpolate(res.y, p_control_1.y, p_control_2.y, p_end.y, p_t);
+	res.z = Math::bezier_interpolate(res.z, p_control_1.z, p_control_2.z, p_end.z, p_t);
+	return res;
+}
+
+Vector3 Vector3::bezier_derivative(const Vector3 &p_control_1, const Vector3 &p_control_2, const Vector3 &p_end, real_t p_t) const {
+	Vector3 res = *this;
+	res.x = Math::bezier_derivative(res.x, p_control_1.x, p_control_2.x, p_end.x, p_t);
+	res.y = Math::bezier_derivative(res.y, p_control_1.y, p_control_2.y, p_end.y, p_t);
+	res.z = Math::bezier_derivative(res.z, p_control_1.z, p_control_2.z, p_end.z, p_t);
+	return res;
+}
+
+real_t Vector3::distance_to(const Vector3 &p_to) const {
+	return (p_to - *this).length();
+}
+
+real_t Vector3::distance_squared_to(const Vector3 &p_to) const {
+	return (p_to - *this).length_squared();
+}
+
+Vector3 Vector3::posmod(real_t p_mod) const {
+	return Vector3(Math::fposmod(x, p_mod), Math::fposmod(y, p_mod), Math::fposmod(z, p_mod));
+}
+
+Vector3 Vector3::posmodv(const Vector3 &p_modv) const {
+	return Vector3(Math::fposmod(x, p_modv.x), Math::fposmod(y, p_modv.y), Math::fposmod(z, p_modv.z));
+}
+
+Vector3 Vector3::project(const Vector3 &p_to) const {
+	return p_to * (dot(p_to) / p_to.length_squared());
+}
+
+real_t Vector3::angle_to(const Vector3 &p_to) const {
+	return Math::atan2(cross(p_to).length(), dot(p_to));
+}
+
+real_t Vector3::signed_angle_to(const Vector3 &p_to, const Vector3 &p_axis) const {
+	Vector3 cross_to = cross(p_to);
+	real_t unsigned_angle = Math::atan2(cross_to.length(), dot(p_to));
+	real_t sign = cross_to.dot(p_axis);
+	return (sign < 0) ? -unsigned_angle : unsigned_angle;
+}
+
+Vector3 Vector3::direction_to(const Vector3 &p_to) const {
+	Vector3 ret(p_to.x - x, p_to.y - y, p_to.z - z);
+	ret.normalize();
+	return ret;
+}
+
+Vector3 Vector3::get_any_perpendicular() const {
+	// Return the any perpendicular vector by cross product with the Vector3.RIGHT or Vector3.UP,
+	// whichever has the greater angle to the current vector with the sign of each element positive.
+	// The only essence is "to avoid being parallel to the current vector", and there is no mathematical basis for using Vector3.RIGHT and Vector3.UP,
+	// since it could be a different vector depending on the prior branching code Math::abs(x) <= Math::abs(y) && Math::abs(x) <= Math::abs(z).
+	// However, it would be reasonable to use any of the axes of the basis, as it is simpler to calculate.
+	ERR_FAIL_COND_V_MSG(is_zero_approx(), Vector3(0, 0, 0), "The Vector3 must not be zero.");
+	return cross((Math::abs(x) <= Math::abs(y) && Math::abs(x) <= Math::abs(z)) ? Vector3(1, 0, 0) : Vector3(0, 1, 0)).normalized();
+}
+
+/* Operators */
+
+constexpr Vector3 &Vector3::operator+=(const Vector3 &p_v) {
+	x += p_v.x;
+	y += p_v.y;
+	z += p_v.z;
+	return *this;
+}
+
+constexpr Vector3 Vector3::operator+(const Vector3 &p_v) const {
+	return Vector3(x + p_v.x, y + p_v.y, z + p_v.z);
+}
+
+constexpr Vector3 &Vector3::operator-=(const Vector3 &p_v) {
+	x -= p_v.x;
+	y -= p_v.y;
+	z -= p_v.z;
+	return *this;
+}
+
+constexpr Vector3 Vector3::operator-(const Vector3 &p_v) const {
+	return Vector3(x - p_v.x, y - p_v.y, z - p_v.z);
+}
+
+constexpr Vector3 &Vector3::operator*=(const Vector3 &p_v) {
+	x *= p_v.x;
+	y *= p_v.y;
+	z *= p_v.z;
+	return *this;
+}
+
+constexpr Vector3 Vector3::operator*(const Vector3 &p_v) const {
+	return Vector3(x * p_v.x, y * p_v.y, z * p_v.z);
+}
+
+constexpr Vector3 &Vector3::operator/=(const Vector3 &p_v) {
+	x /= p_v.x;
+	y /= p_v.y;
+	z /= p_v.z;
+	return *this;
+}
+
+constexpr Vector3 Vector3::operator/(const Vector3 &p_v) const {
+	return Vector3(x / p_v.x, y / p_v.y, z / p_v.z);
+}
+
+constexpr Vector3 &Vector3::operator*=(real_t p_scalar) {
+	x *= p_scalar;
+	y *= p_scalar;
+	z *= p_scalar;
+	return *this;
+}
+
+// Multiplication operators required to workaround issues with LLVM using implicit conversion
+// to Vector3i instead for integers where it should not.
+
+constexpr Vector3 operator*(float p_scalar, const Vector3 &p_vec) {
+	return p_vec * p_scalar;
+}
+
+constexpr Vector3 operator*(double p_scalar, const Vector3 &p_vec) {
+	return p_vec * p_scalar;
+}
+
+constexpr Vector3 operator*(int32_t p_scalar, const Vector3 &p_vec) {
+	return p_vec * p_scalar;
+}
+
+constexpr Vector3 operator*(int64_t p_scalar, const Vector3 &p_vec) {
+	return p_vec * p_scalar;
+}
+
+constexpr Vector3 Vector3::operator*(real_t p_scalar) const {
+	return Vector3(x * p_scalar, y * p_scalar, z * p_scalar);
+}
+
+constexpr Vector3 &Vector3::operator/=(real_t p_scalar) {
+	x /= p_scalar;
+	y /= p_scalar;
+	z /= p_scalar;
+	return *this;
+}
+
+constexpr Vector3 Vector3::operator/(real_t p_scalar) const {
+	return Vector3(x / p_scalar, y / p_scalar, z / p_scalar);
+}
+
+constexpr Vector3 Vector3::operator-() const {
+	return Vector3(-x, -y, -z);
+}
+
+constexpr bool Vector3::operator==(const Vector3 &p_v) const {
+	return x == p_v.x && y == p_v.y && z == p_v.z;
+}
+
+constexpr bool Vector3::operator!=(const Vector3 &p_v) const {
+	return x != p_v.x || y != p_v.y || z != p_v.z;
+}
+
+constexpr bool Vector3::operator<(const Vector3 &p_v) const {
+	if (x == p_v.x) {
+		if (y == p_v.y) {
+			return z < p_v.z;
+		}
+		return y < p_v.y;
+	}
+	return x < p_v.x;
+}
+
+constexpr bool Vector3::operator>(const Vector3 &p_v) const {
+	if (x == p_v.x) {
+		if (y == p_v.y) {
+			return z > p_v.z;
+		}
+		return y > p_v.y;
+	}
+	return x > p_v.x;
+}
+
+constexpr bool Vector3::operator<=(const Vector3 &p_v) const {
+	if (x == p_v.x) {
+		if (y == p_v.y) {
+			return z <= p_v.z;
+		}
+		return y < p_v.y;
+	}
+	return x < p_v.x;
+}
+
+constexpr bool Vector3::operator>=(const Vector3 &p_v) const {
+	if (x == p_v.x) {
+		if (y == p_v.y) {
+			return z >= p_v.z;
+		}
+		return y > p_v.y;
+	}
+	return x > p_v.x;
+}
+
+_FORCE_INLINE_ Vector3 vec3_cross(const Vector3 &p_a, const Vector3 &p_b) {
+	return p_a.cross(p_b);
+}
+
+_FORCE_INLINE_ real_t vec3_dot(const Vector3 &p_a, const Vector3 &p_b) {
+	return p_a.dot(p_b);
+}
+
+real_t Vector3::length() const {
+	real_t x2 = x * x;
+	real_t y2 = y * y;
+	real_t z2 = z * z;
+
+	return Math::sqrt(x2 + y2 + z2);
+}
+
+real_t Vector3::length_squared() const {
+	real_t x2 = x * x;
+	real_t y2 = y * y;
+	real_t z2 = z * z;
+
+	return x2 + y2 + z2;
+}
+
+void Vector3::normalize() {
+	real_t lengthsq = length_squared();
+	if (lengthsq == 0) {
+		x = y = z = 0;
+	} else {
+		real_t length = Math::sqrt(lengthsq);
+		x /= length;
+		y /= length;
+		z /= length;
+	}
+}
+
+Vector3 Vector3::normalized() const {
+	Vector3 v = *this;
+	v.normalize();
+	return v;
+}
+
+bool Vector3::is_normalized() const {
+	// use length_squared() instead of length() to avoid sqrt(), makes it more stringent.
+	return Math::is_equal_approx(length_squared(), 1, (real_t)UNIT_EPSILON);
+}
+
+Vector3 Vector3::inverse() const {
+	return Vector3(1.0f / x, 1.0f / y, 1.0f / z);
+}
+
+void Vector3::zero() {
+	x = y = z = 0;
+}
+
+// slide returns the component of the vector along the given plane, specified by its normal vector.
+Vector3 Vector3::slide(const Vector3 &p_normal) const {
+#ifdef MATH_CHECKS
+	ERR_FAIL_COND_V_MSG(!p_normal.is_normalized(), Vector3(), "The normal Vector3 " + p_normal.operator String() + " must be normalized.");
+#endif
+	return *this - p_normal * dot(p_normal);
+}
+
+Vector3 Vector3::bounce(const Vector3 &p_normal) const {
+	return -reflect(p_normal);
+}
+
+Vector3 Vector3::reflect(const Vector3 &p_normal) const {
+#ifdef MATH_CHECKS
+	ERR_FAIL_COND_V_MSG(!p_normal.is_normalized(), Vector3(), "The normal Vector3 " + p_normal.operator String() + " must be normalized.");
+#endif
+	return 2.0f * p_normal * dot(p_normal) - *this;
+}
+
+template <>
+struct is_zero_constructible<Vector3> : std::true_type {};

core/math/vector3i.cpp

@@ -0,0 +1,78 @@
+/**************************************************************************/
+/*  vector3i.cpp                                                          */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "vector3i.h"
+
+#include "core/math/vector3.h"
+#include "core/string/ustring.h"
+
+Vector3i::Axis Vector3i::min_axis_index() const {
+	return x < y ? (x < z ? Vector3i::AXIS_X : Vector3i::AXIS_Z) : (y < z ? Vector3i::AXIS_Y : Vector3i::AXIS_Z);
+}
+
+Vector3i::Axis Vector3i::max_axis_index() const {
+	return x < y ? (y < z ? Vector3i::AXIS_Z : Vector3i::AXIS_Y) : (x < z ? Vector3i::AXIS_Z : Vector3i::AXIS_X);
+}
+
+Vector3i Vector3i::clamp(const Vector3i &p_min, const Vector3i &p_max) const {
+	return Vector3i(
+			CLAMP(x, p_min.x, p_max.x),
+			CLAMP(y, p_min.y, p_max.y),
+			CLAMP(z, p_min.z, p_max.z));
+}
+
+Vector3i Vector3i::clampi(int32_t p_min, int32_t p_max) const {
+	return Vector3i(
+			CLAMP(x, p_min, p_max),
+			CLAMP(y, p_min, p_max),
+			CLAMP(z, p_min, p_max));
+}
+
+Vector3i Vector3i::snapped(const Vector3i &p_step) const {
+	return Vector3i(
+			Math::snapped(x, p_step.x),
+			Math::snapped(y, p_step.y),
+			Math::snapped(z, p_step.z));
+}
+
+Vector3i Vector3i::snappedi(int32_t p_step) const {
+	return Vector3i(
+			Math::snapped(x, p_step),
+			Math::snapped(y, p_step),
+			Math::snapped(z, p_step));
+}
+
+Vector3i::operator String() const {
+	return "(" + itos(x) + ", " + itos(y) + ", " + itos(z) + ")";
+}
+
+Vector3i::operator Vector3() const {
+	return Vector3(x, y, z);
+}

core/math/vector3i.h

@@ -0,0 +1,339 @@
+/**************************************************************************/
+/*  vector3i.h                                                            */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "core/error/error_macros.h"
+#include "core/math/math_funcs.h"
+
+class String;
+struct Vector3;
+
+struct [[nodiscard]] Vector3i {
+	static const int AXIS_COUNT = 3;
+
+	enum Axis {
+		AXIS_X,
+		AXIS_Y,
+		AXIS_Z,
+	};
+
+	union {
+		// NOLINTBEGIN(modernize-use-default-member-init)
+		struct {
+			int32_t x;
+			int32_t y;
+			int32_t z;
+		};
+
+		int32_t coord[3] = { 0 };
+		// NOLINTEND(modernize-use-default-member-init)
+	};
+
+	_FORCE_INLINE_ const int32_t &operator[](int p_axis) const {
+		DEV_ASSERT((unsigned int)p_axis < 3);
+		return coord[p_axis];
+	}
+
+	_FORCE_INLINE_ int32_t &operator[](int p_axis) {
+		DEV_ASSERT((unsigned int)p_axis < 3);
+		return coord[p_axis];
+	}
+
+	Vector3i::Axis min_axis_index() const;
+	Vector3i::Axis max_axis_index() const;
+
+	Vector3i min(const Vector3i &p_vector3i) const {
+		return Vector3i(MIN(x, p_vector3i.x), MIN(y, p_vector3i.y), MIN(z, p_vector3i.z));
+	}
+
+	Vector3i mini(int32_t p_scalar) const {
+		return Vector3i(MIN(x, p_scalar), MIN(y, p_scalar), MIN(z, p_scalar));
+	}
+
+	Vector3i max(const Vector3i &p_vector3i) const {
+		return Vector3i(MAX(x, p_vector3i.x), MAX(y, p_vector3i.y), MAX(z, p_vector3i.z));
+	}
+
+	Vector3i maxi(int32_t p_scalar) const {
+		return Vector3i(MAX(x, p_scalar), MAX(y, p_scalar), MAX(z, p_scalar));
+	}
+
+	_FORCE_INLINE_ int64_t length_squared() const;
+	_FORCE_INLINE_ double length() const;
+
+	_FORCE_INLINE_ void zero();
+
+	_FORCE_INLINE_ Vector3i abs() const;
+	_FORCE_INLINE_ Vector3i sign() const;
+	Vector3i clamp(const Vector3i &p_min, const Vector3i &p_max) const;
+	Vector3i clampi(int32_t p_min, int32_t p_max) const;
+	Vector3i snapped(const Vector3i &p_step) const;
+	Vector3i snappedi(int32_t p_step) const;
+
+	_FORCE_INLINE_ double distance_to(const Vector3i &p_to) const;
+	_FORCE_INLINE_ int64_t distance_squared_to(const Vector3i &p_to) const;
+
+	/* Operators */
+
+	constexpr Vector3i &operator+=(const Vector3i &p_v);
+	constexpr Vector3i operator+(const Vector3i &p_v) const;
+	constexpr Vector3i &operator-=(const Vector3i &p_v);
+	constexpr Vector3i operator-(const Vector3i &p_v) const;
+	constexpr Vector3i &operator*=(const Vector3i &p_v);
+	constexpr Vector3i operator*(const Vector3i &p_v) const;
+	constexpr Vector3i &operator/=(const Vector3i &p_v);
+	constexpr Vector3i operator/(const Vector3i &p_v) const;
+	constexpr Vector3i &operator%=(const Vector3i &p_v);
+	constexpr Vector3i operator%(const Vector3i &p_v) const;
+
+	constexpr Vector3i &operator*=(int32_t p_scalar);
+	constexpr Vector3i operator*(int32_t p_scalar) const;
+	constexpr Vector3i &operator/=(int32_t p_scalar);
+	constexpr Vector3i operator/(int32_t p_scalar) const;
+	constexpr Vector3i &operator%=(int32_t p_scalar);
+	constexpr Vector3i operator%(int32_t p_scalar) const;
+
+	constexpr Vector3i operator-() const;
+
+	constexpr bool operator==(const Vector3i &p_v) const;
+	constexpr bool operator!=(const Vector3i &p_v) const;
+	constexpr bool operator<(const Vector3i &p_v) const;
+	constexpr bool operator<=(const Vector3i &p_v) const;
+	constexpr bool operator>(const Vector3i &p_v) const;
+	constexpr bool operator>=(const Vector3i &p_v) const;
+
+	explicit operator String() const;
+	operator Vector3() const;
+
+	constexpr Vector3i() :
+			x(0), y(0), z(0) {}
+	constexpr Vector3i(int32_t p_x, int32_t p_y, int32_t p_z) :
+			x(p_x), y(p_y), z(p_z) {}
+};
+
+int64_t Vector3i::length_squared() const {
+	return x * (int64_t)x + y * (int64_t)y + z * (int64_t)z;
+}
+
+double Vector3i::length() const {
+	return Math::sqrt((double)length_squared());
+}
+
+Vector3i Vector3i::abs() const {
+	return Vector3i(Math::abs(x), Math::abs(y), Math::abs(z));
+}
+
+Vector3i Vector3i::sign() const {
+	return Vector3i(SIGN(x), SIGN(y), SIGN(z));
+}
+
+double Vector3i::distance_to(const Vector3i &p_to) const {
+	return (p_to - *this).length();
+}
+
+int64_t Vector3i::distance_squared_to(const Vector3i &p_to) const {
+	return (p_to - *this).length_squared();
+}
+
+/* Operators */
+
+constexpr Vector3i &Vector3i::operator+=(const Vector3i &p_v) {
+	x += p_v.x;
+	y += p_v.y;
+	z += p_v.z;
+	return *this;
+}
+
+constexpr Vector3i Vector3i::operator+(const Vector3i &p_v) const {
+	return Vector3i(x + p_v.x, y + p_v.y, z + p_v.z);
+}
+
+constexpr Vector3i &Vector3i::operator-=(const Vector3i &p_v) {
+	x -= p_v.x;
+	y -= p_v.y;
+	z -= p_v.z;
+	return *this;
+}
+
+constexpr Vector3i Vector3i::operator-(const Vector3i &p_v) const {
+	return Vector3i(x - p_v.x, y - p_v.y, z - p_v.z);
+}
+
+constexpr Vector3i &Vector3i::operator*=(const Vector3i &p_v) {
+	x *= p_v.x;
+	y *= p_v.y;
+	z *= p_v.z;
+	return *this;
+}
+
+constexpr Vector3i Vector3i::operator*(const Vector3i &p_v) const {
+	return Vector3i(x * p_v.x, y * p_v.y, z * p_v.z);
+}
+
+constexpr Vector3i &Vector3i::operator/=(const Vector3i &p_v) {
+	x /= p_v.x;
+	y /= p_v.y;
+	z /= p_v.z;
+	return *this;
+}
+
+constexpr Vector3i Vector3i::operator/(const Vector3i &p_v) const {
+	return Vector3i(x / p_v.x, y / p_v.y, z / p_v.z);
+}
+
+constexpr Vector3i &Vector3i::operator%=(const Vector3i &p_v) {
+	x %= p_v.x;
+	y %= p_v.y;
+	z %= p_v.z;
+	return *this;
+}
+
+constexpr Vector3i Vector3i::operator%(const Vector3i &p_v) const {
+	return Vector3i(x % p_v.x, y % p_v.y, z % p_v.z);
+}
+
+constexpr Vector3i &Vector3i::operator*=(int32_t p_scalar) {
+	x *= p_scalar;
+	y *= p_scalar;
+	z *= p_scalar;
+	return *this;
+}
+
+constexpr Vector3i Vector3i::operator*(int32_t p_scalar) const {
+	return Vector3i(x * p_scalar, y * p_scalar, z * p_scalar);
+}
+
+// Multiplication operators required to workaround issues with LLVM using implicit conversion.
+
+constexpr Vector3i operator*(int32_t p_scalar, const Vector3i &p_vector) {
+	return p_vector * p_scalar;
+}
+
+constexpr Vector3i operator*(int64_t p_scalar, const Vector3i &p_vector) {
+	return p_vector * p_scalar;
+}
+
+constexpr Vector3i operator*(float p_scalar, const Vector3i &p_vector) {
+	return p_vector * p_scalar;
+}
+
+constexpr Vector3i operator*(double p_scalar, const Vector3i &p_vector) {
+	return p_vector * p_scalar;
+}
+
+constexpr Vector3i &Vector3i::operator/=(int32_t p_scalar) {
+	x /= p_scalar;
+	y /= p_scalar;
+	z /= p_scalar;
+	return *this;
+}
+
+constexpr Vector3i Vector3i::operator/(int32_t p_scalar) const {
+	return Vector3i(x / p_scalar, y / p_scalar, z / p_scalar);
+}
+
+constexpr Vector3i &Vector3i::operator%=(int32_t p_scalar) {
+	x %= p_scalar;
+	y %= p_scalar;
+	z %= p_scalar;
+	return *this;
+}
+
+constexpr Vector3i Vector3i::operator%(int32_t p_scalar) const {
+	return Vector3i(x % p_scalar, y % p_scalar, z % p_scalar);
+}
+
+constexpr Vector3i Vector3i::operator-() const {
+	return Vector3i(-x, -y, -z);
+}
+
+constexpr bool Vector3i::operator==(const Vector3i &p_v) const {
+	return (x == p_v.x && y == p_v.y && z == p_v.z);
+}
+
+constexpr bool Vector3i::operator!=(const Vector3i &p_v) const {
+	return (x != p_v.x || y != p_v.y || z != p_v.z);
+}
+
+constexpr bool Vector3i::operator<(const Vector3i &p_v) const {
+	if (x == p_v.x) {
+		if (y == p_v.y) {
+			return z < p_v.z;
+		} else {
+			return y < p_v.y;
+		}
+	} else {
+		return x < p_v.x;
+	}
+}
+
+constexpr bool Vector3i::operator>(const Vector3i &p_v) const {
+	if (x == p_v.x) {
+		if (y == p_v.y) {
+			return z > p_v.z;
+		} else {
+			return y > p_v.y;
+		}
+	} else {
+		return x > p_v.x;
+	}
+}
+
+constexpr bool Vector3i::operator<=(const Vector3i &p_v) const {
+	if (x == p_v.x) {
+		if (y == p_v.y) {
+			return z <= p_v.z;
+		} else {
+			return y < p_v.y;
+		}
+	} else {
+		return x < p_v.x;
+	}
+}
+
+constexpr bool Vector3i::operator>=(const Vector3i &p_v) const {
+	if (x == p_v.x) {
+		if (y == p_v.y) {
+			return z >= p_v.z;
+		} else {
+			return y > p_v.y;
+		}
+	} else {
+		return x > p_v.x;
+	}
+}
+
+void Vector3i::zero() {
+	x = y = z = 0;
+}
+
+template <>
+struct is_zero_constructible<Vector3i> : std::true_type {};

core/math/vector4.cpp

@@ -0,0 +1,227 @@
+/**************************************************************************/
+/*  vector4.cpp                                                           */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "vector4.h"
+
+#include "core/math/math_funcs.h"
+#include "core/math/vector4i.h"
+#include "core/string/ustring.h"
+
+Vector4::Axis Vector4::min_axis_index() const {
+	uint32_t min_index = 0;
+	real_t min_value = x;
+	for (uint32_t i = 1; i < 4; i++) {
+		if (operator[](i) <= min_value) {
+			min_index = i;
+			min_value = operator[](i);
+		}
+	}
+	return Vector4::Axis(min_index);
+}
+
+Vector4::Axis Vector4::max_axis_index() const {
+	uint32_t max_index = 0;
+	real_t max_value = x;
+	for (uint32_t i = 1; i < 4; i++) {
+		if (operator[](i) > max_value) {
+			max_index = i;
+			max_value = operator[](i);
+		}
+	}
+	return Vector4::Axis(max_index);
+}
+
+bool Vector4::is_equal_approx(const Vector4 &p_vec4) const {
+	return Math::is_equal_approx(x, p_vec4.x) && Math::is_equal_approx(y, p_vec4.y) && Math::is_equal_approx(z, p_vec4.z) && Math::is_equal_approx(w, p_vec4.w);
+}
+
+bool Vector4::is_same(const Vector4 &p_vec4) const {
+	return Math::is_same(x, p_vec4.x) && Math::is_same(y, p_vec4.y) && Math::is_same(z, p_vec4.z) && Math::is_same(w, p_vec4.w);
+}
+
+bool Vector4::is_zero_approx() const {
+	return Math::is_zero_approx(x) && Math::is_zero_approx(y) && Math::is_zero_approx(z) && Math::is_zero_approx(w);
+}
+
+bool Vector4::is_finite() const {
+	return Math::is_finite(x) && Math::is_finite(y) && Math::is_finite(z) && Math::is_finite(w);
+}
+
+real_t Vector4::length() const {
+	return Math::sqrt(length_squared());
+}
+
+void Vector4::normalize() {
+	real_t lengthsq = length_squared();
+	if (lengthsq == 0) {
+		x = y = z = w = 0;
+	} else {
+		real_t length = Math::sqrt(lengthsq);
+		x /= length;
+		y /= length;
+		z /= length;
+		w /= length;
+	}
+}
+
+Vector4 Vector4::normalized() const {
+	Vector4 v = *this;
+	v.normalize();
+	return v;
+}
+
+bool Vector4::is_normalized() const {
+	return Math::is_equal_approx(length_squared(), (real_t)1, (real_t)UNIT_EPSILON);
+}
+
+real_t Vector4::distance_to(const Vector4 &p_to) const {
+	return (p_to - *this).length();
+}
+
+real_t Vector4::distance_squared_to(const Vector4 &p_to) const {
+	return (p_to - *this).length_squared();
+}
+
+Vector4 Vector4::direction_to(const Vector4 &p_to) const {
+	Vector4 ret(p_to.x - x, p_to.y - y, p_to.z - z, p_to.w - w);
+	ret.normalize();
+	return ret;
+}
+
+Vector4 Vector4::abs() const {
+	return Vector4(Math::abs(x), Math::abs(y), Math::abs(z), Math::abs(w));
+}
+
+Vector4 Vector4::sign() const {
+	return Vector4(SIGN(x), SIGN(y), SIGN(z), SIGN(w));
+}
+
+Vector4 Vector4::floor() const {
+	return Vector4(Math::floor(x), Math::floor(y), Math::floor(z), Math::floor(w));
+}
+
+Vector4 Vector4::ceil() const {
+	return Vector4(Math::ceil(x), Math::ceil(y), Math::ceil(z), Math::ceil(w));
+}
+
+Vector4 Vector4::round() const {
+	return Vector4(Math::round(x), Math::round(y), Math::round(z), Math::round(w));
+}
+
+Vector4 Vector4::lerp(const Vector4 &p_to, real_t p_weight) const {
+	Vector4 res = *this;
+	res.x = Math::lerp(res.x, p_to.x, p_weight);
+	res.y = Math::lerp(res.y, p_to.y, p_weight);
+	res.z = Math::lerp(res.z, p_to.z, p_weight);
+	res.w = Math::lerp(res.w, p_to.w, p_weight);
+	return res;
+}
+
+Vector4 Vector4::cubic_interpolate(const Vector4 &p_b, const Vector4 &p_pre_a, const Vector4 &p_post_b, real_t p_weight) const {
+	Vector4 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);
+	res.w = Math::cubic_interpolate(res.w, p_b.w, p_pre_a.w, p_post_b.w, p_weight);
+	return res;
+}
+
+Vector4 Vector4::cubic_interpolate_in_time(const Vector4 &p_b, const Vector4 &p_pre_a, const Vector4 &p_post_b, real_t p_weight, real_t p_b_t, real_t p_pre_a_t, real_t p_post_b_t) const {
+	Vector4 res = *this;
+	res.x = Math::cubic_interpolate_in_time(res.x, p_b.x, p_pre_a.x, p_post_b.x, p_weight, p_b_t, p_pre_a_t, p_post_b_t);
+	res.y = Math::cubic_interpolate_in_time(res.y, p_b.y, p_pre_a.y, p_post_b.y, p_weight, p_b_t, p_pre_a_t, p_post_b_t);
+	res.z = Math::cubic_interpolate_in_time(res.z, p_b.z, p_pre_a.z, p_post_b.z, p_weight, p_b_t, p_pre_a_t, p_post_b_t);
+	res.w = Math::cubic_interpolate_in_time(res.w, p_b.w, p_pre_a.w, p_post_b.w, p_weight, p_b_t, p_pre_a_t, p_post_b_t);
+	return res;
+}
+
+Vector4 Vector4::posmod(real_t p_mod) const {
+	return Vector4(Math::fposmod(x, p_mod), Math::fposmod(y, p_mod), Math::fposmod(z, p_mod), Math::fposmod(w, p_mod));
+}
+
+Vector4 Vector4::posmodv(const Vector4 &p_modv) const {
+	return Vector4(Math::fposmod(x, p_modv.x), Math::fposmod(y, p_modv.y), Math::fposmod(z, p_modv.z), Math::fposmod(w, p_modv.w));
+}
+
+void Vector4::snap(const Vector4 &p_step) {
+	x = Math::snapped(x, p_step.x);
+	y = Math::snapped(y, p_step.y);
+	z = Math::snapped(z, p_step.z);
+	w = Math::snapped(w, p_step.w);
+}
+
+void Vector4::snapf(real_t p_step) {
+	x = Math::snapped(x, p_step);
+	y = Math::snapped(y, p_step);
+	z = Math::snapped(z, p_step);
+	w = Math::snapped(w, p_step);
+}
+
+Vector4 Vector4::snapped(const Vector4 &p_step) const {
+	Vector4 v = *this;
+	v.snap(p_step);
+	return v;
+}
+
+Vector4 Vector4::snappedf(real_t p_step) const {
+	Vector4 v = *this;
+	v.snapf(p_step);
+	return v;
+}
+
+Vector4 Vector4::inverse() const {
+	return Vector4(1.0f / x, 1.0f / y, 1.0f / z, 1.0f / w);
+}
+
+Vector4 Vector4::clamp(const Vector4 &p_min, const Vector4 &p_max) const {
+	return Vector4(
+			CLAMP(x, p_min.x, p_max.x),
+			CLAMP(y, p_min.y, p_max.y),
+			CLAMP(z, p_min.z, p_max.z),
+			CLAMP(w, p_min.w, p_max.w));
+}
+
+Vector4 Vector4::clampf(real_t p_min, real_t p_max) const {
+	return Vector4(
+			CLAMP(x, p_min, p_max),
+			CLAMP(y, p_min, p_max),
+			CLAMP(z, p_min, p_max),
+			CLAMP(w, p_min, p_max));
+}
+
+Vector4::operator String() const {
+	return "(" + String::num_real(x, true) + ", " + String::num_real(y, true) + ", " + String::num_real(z, true) + ", " + String::num_real(w, true) + ")";
+}
+
+static_assert(sizeof(Vector4) == 4 * sizeof(real_t));
+
+Vector4::operator Vector4i() const {
+	return Vector4i(x, y, z, w);
+}

core/math/vector4.h

@@ -0,0 +1,306 @@
+/**************************************************************************/
+/*  vector4.h                                                             */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "core/error/error_macros.h"
+#include "core/math/math_defs.h"
+#include "core/typedefs.h"
+
+class String;
+struct Vector4i;
+
+struct [[nodiscard]] Vector4 {
+	static const int AXIS_COUNT = 4;
+
+	enum Axis {
+		AXIS_X,
+		AXIS_Y,
+		AXIS_Z,
+		AXIS_W,
+	};
+
+	union {
+		// NOLINTBEGIN(modernize-use-default-member-init)
+		struct {
+			real_t x;
+			real_t y;
+			real_t z;
+			real_t w;
+		};
+		real_t coord[4] = { 0, 0, 0, 0 };
+		// NOLINTEND(modernize-use-default-member-init)
+	};
+
+	_FORCE_INLINE_ real_t &operator[](int p_axis) {
+		DEV_ASSERT((unsigned int)p_axis < 4);
+		return coord[p_axis];
+	}
+	_FORCE_INLINE_ const real_t &operator[](int p_axis) const {
+		DEV_ASSERT((unsigned int)p_axis < 4);
+		return coord[p_axis];
+	}
+
+	Vector4::Axis min_axis_index() const;
+	Vector4::Axis max_axis_index() const;
+
+	Vector4 min(const Vector4 &p_vector4) const {
+		return Vector4(MIN(x, p_vector4.x), MIN(y, p_vector4.y), MIN(z, p_vector4.z), MIN(w, p_vector4.w));
+	}
+
+	Vector4 minf(real_t p_scalar) const {
+		return Vector4(MIN(x, p_scalar), MIN(y, p_scalar), MIN(z, p_scalar), MIN(w, p_scalar));
+	}
+
+	Vector4 max(const Vector4 &p_vector4) const {
+		return Vector4(MAX(x, p_vector4.x), MAX(y, p_vector4.y), MAX(z, p_vector4.z), MAX(w, p_vector4.w));
+	}
+
+	Vector4 maxf(real_t p_scalar) const {
+		return Vector4(MAX(x, p_scalar), MAX(y, p_scalar), MAX(z, p_scalar), MAX(w, p_scalar));
+	}
+
+	_FORCE_INLINE_ real_t length_squared() const;
+	bool is_equal_approx(const Vector4 &p_vec4) const;
+	bool is_zero_approx() const;
+	bool is_same(const Vector4 &p_vec4) const;
+	bool is_finite() const;
+	real_t length() const;
+	void normalize();
+	Vector4 normalized() const;
+	bool is_normalized() const;
+
+	real_t distance_to(const Vector4 &p_to) const;
+	real_t distance_squared_to(const Vector4 &p_to) const;
+	Vector4 direction_to(const Vector4 &p_to) const;
+
+	Vector4 abs() const;
+	Vector4 sign() const;
+	Vector4 floor() const;
+	Vector4 ceil() const;
+	Vector4 round() const;
+	Vector4 lerp(const Vector4 &p_to, real_t p_weight) const;
+	Vector4 cubic_interpolate(const Vector4 &p_b, const Vector4 &p_pre_a, const Vector4 &p_post_b, real_t p_weight) const;
+	Vector4 cubic_interpolate_in_time(const Vector4 &p_b, const Vector4 &p_pre_a, const Vector4 &p_post_b, real_t p_weight, real_t p_b_t, real_t p_pre_a_t, real_t p_post_b_t) const;
+
+	Vector4 posmod(real_t p_mod) const;
+	Vector4 posmodv(const Vector4 &p_modv) const;
+	void snap(const Vector4 &p_step);
+	void snapf(real_t p_step);
+	Vector4 snapped(const Vector4 &p_step) const;
+	Vector4 snappedf(real_t p_step) const;
+	Vector4 clamp(const Vector4 &p_min, const Vector4 &p_max) const;
+	Vector4 clampf(real_t p_min, real_t p_max) const;
+
+	Vector4 inverse() const;
+	_FORCE_INLINE_ real_t dot(const Vector4 &p_vec4) const;
+
+	constexpr void operator+=(const Vector4 &p_vec4);
+	constexpr void operator-=(const Vector4 &p_vec4);
+	constexpr void operator*=(const Vector4 &p_vec4);
+	constexpr void operator/=(const Vector4 &p_vec4);
+	constexpr void operator*=(real_t p_s);
+	constexpr void operator/=(real_t p_s);
+	constexpr Vector4 operator+(const Vector4 &p_vec4) const;
+	constexpr Vector4 operator-(const Vector4 &p_vec4) const;
+	constexpr Vector4 operator*(const Vector4 &p_vec4) const;
+	constexpr Vector4 operator/(const Vector4 &p_vec4) const;
+	constexpr Vector4 operator-() const;
+	constexpr Vector4 operator*(real_t p_s) const;
+	constexpr Vector4 operator/(real_t p_s) const;
+
+	constexpr bool operator==(const Vector4 &p_vec4) const;
+	constexpr bool operator!=(const Vector4 &p_vec4) const;
+	constexpr bool operator>(const Vector4 &p_vec4) const;
+	constexpr bool operator<(const Vector4 &p_vec4) const;
+	constexpr bool operator>=(const Vector4 &p_vec4) const;
+	constexpr bool operator<=(const Vector4 &p_vec4) const;
+
+	explicit operator String() const;
+	operator Vector4i() const;
+
+	constexpr Vector4() :
+			x(0), y(0), z(0), w(0) {}
+	constexpr Vector4(real_t p_x, real_t p_y, real_t p_z, real_t p_w) :
+			x(p_x), y(p_y), z(p_z), w(p_w) {}
+};
+
+real_t Vector4::dot(const Vector4 &p_vec4) const {
+	return x * p_vec4.x + y * p_vec4.y + z * p_vec4.z + w * p_vec4.w;
+}
+
+real_t Vector4::length_squared() const {
+	return dot(*this);
+}
+
+constexpr void Vector4::operator+=(const Vector4 &p_vec4) {
+	x += p_vec4.x;
+	y += p_vec4.y;
+	z += p_vec4.z;
+	w += p_vec4.w;
+}
+
+constexpr void Vector4::operator-=(const Vector4 &p_vec4) {
+	x -= p_vec4.x;
+	y -= p_vec4.y;
+	z -= p_vec4.z;
+	w -= p_vec4.w;
+}
+
+constexpr void Vector4::operator*=(const Vector4 &p_vec4) {
+	x *= p_vec4.x;
+	y *= p_vec4.y;
+	z *= p_vec4.z;
+	w *= p_vec4.w;
+}
+
+constexpr void Vector4::operator/=(const Vector4 &p_vec4) {
+	x /= p_vec4.x;
+	y /= p_vec4.y;
+	z /= p_vec4.z;
+	w /= p_vec4.w;
+}
+constexpr void Vector4::operator*=(real_t p_s) {
+	x *= p_s;
+	y *= p_s;
+	z *= p_s;
+	w *= p_s;
+}
+
+constexpr void Vector4::operator/=(real_t p_s) {
+	*this *= (1 / p_s);
+}
+
+constexpr Vector4 Vector4::operator+(const Vector4 &p_vec4) const {
+	return Vector4(x + p_vec4.x, y + p_vec4.y, z + p_vec4.z, w + p_vec4.w);
+}
+
+constexpr Vector4 Vector4::operator-(const Vector4 &p_vec4) const {
+	return Vector4(x - p_vec4.x, y - p_vec4.y, z - p_vec4.z, w - p_vec4.w);
+}
+
+constexpr Vector4 Vector4::operator*(const Vector4 &p_vec4) const {
+	return Vector4(x * p_vec4.x, y * p_vec4.y, z * p_vec4.z, w * p_vec4.w);
+}
+
+constexpr Vector4 Vector4::operator/(const Vector4 &p_vec4) const {
+	return Vector4(x / p_vec4.x, y / p_vec4.y, z / p_vec4.z, w / p_vec4.w);
+}
+
+constexpr Vector4 Vector4::operator-() const {
+	return Vector4(-x, -y, -z, -w);
+}
+
+constexpr Vector4 Vector4::operator*(real_t p_s) const {
+	return Vector4(x * p_s, y * p_s, z * p_s, w * p_s);
+}
+
+constexpr Vector4 Vector4::operator/(real_t p_s) const {
+	return *this * (1 / p_s);
+}
+
+constexpr bool Vector4::operator==(const Vector4 &p_vec4) const {
+	return x == p_vec4.x && y == p_vec4.y && z == p_vec4.z && w == p_vec4.w;
+}
+
+constexpr bool Vector4::operator!=(const Vector4 &p_vec4) const {
+	return x != p_vec4.x || y != p_vec4.y || z != p_vec4.z || w != p_vec4.w;
+}
+
+constexpr bool Vector4::operator<(const Vector4 &p_v) const {
+	if (x == p_v.x) {
+		if (y == p_v.y) {
+			if (z == p_v.z) {
+				return w < p_v.w;
+			}
+			return z < p_v.z;
+		}
+		return y < p_v.y;
+	}
+	return x < p_v.x;
+}
+
+constexpr bool Vector4::operator>(const Vector4 &p_v) const {
+	if (x == p_v.x) {
+		if (y == p_v.y) {
+			if (z == p_v.z) {
+				return w > p_v.w;
+			}
+			return z > p_v.z;
+		}
+		return y > p_v.y;
+	}
+	return x > p_v.x;
+}
+
+constexpr bool Vector4::operator<=(const Vector4 &p_v) const {
+	if (x == p_v.x) {
+		if (y == p_v.y) {
+			if (z == p_v.z) {
+				return w <= p_v.w;
+			}
+			return z < p_v.z;
+		}
+		return y < p_v.y;
+	}
+	return x < p_v.x;
+}
+
+constexpr bool Vector4::operator>=(const Vector4 &p_v) const {
+	if (x == p_v.x) {
+		if (y == p_v.y) {
+			if (z == p_v.z) {
+				return w >= p_v.w;
+			}
+			return z > p_v.z;
+		}
+		return y > p_v.y;
+	}
+	return x > p_v.x;
+}
+
+constexpr Vector4 operator*(float p_scalar, const Vector4 &p_vec) {
+	return p_vec * p_scalar;
+}
+
+constexpr Vector4 operator*(double p_scalar, const Vector4 &p_vec) {
+	return p_vec * p_scalar;
+}
+
+constexpr Vector4 operator*(int32_t p_scalar, const Vector4 &p_vec) {
+	return p_vec * p_scalar;
+}
+
+constexpr Vector4 operator*(int64_t p_scalar, const Vector4 &p_vec) {
+	return p_vec * p_scalar;
+}
+
+template <>
+struct is_zero_constructible<Vector4> : std::true_type {};

core/math/vector4i.cpp

@@ -0,0 +1,107 @@
+/**************************************************************************/
+/*  vector4i.cpp                                                          */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "vector4i.h"
+
+#include "core/math/vector4.h"
+#include "core/string/ustring.h"
+
+Vector4i::Axis Vector4i::min_axis_index() const {
+	uint32_t min_index = 0;
+	int32_t min_value = x;
+	for (uint32_t i = 1; i < 4; i++) {
+		if (operator[](i) <= min_value) {
+			min_index = i;
+			min_value = operator[](i);
+		}
+	}
+	return Vector4i::Axis(min_index);
+}
+
+Vector4i::Axis Vector4i::max_axis_index() const {
+	uint32_t max_index = 0;
+	int32_t max_value = x;
+	for (uint32_t i = 1; i < 4; i++) {
+		if (operator[](i) > max_value) {
+			max_index = i;
+			max_value = operator[](i);
+		}
+	}
+	return Vector4i::Axis(max_index);
+}
+
+Vector4i Vector4i::clamp(const Vector4i &p_min, const Vector4i &p_max) const {
+	return Vector4i(
+			CLAMP(x, p_min.x, p_max.x),
+			CLAMP(y, p_min.y, p_max.y),
+			CLAMP(z, p_min.z, p_max.z),
+			CLAMP(w, p_min.w, p_max.w));
+}
+
+Vector4i Vector4i::clampi(int32_t p_min, int32_t p_max) const {
+	return Vector4i(
+			CLAMP(x, p_min, p_max),
+			CLAMP(y, p_min, p_max),
+			CLAMP(z, p_min, p_max),
+			CLAMP(w, p_min, p_max));
+}
+
+Vector4i Vector4i::snapped(const Vector4i &p_step) const {
+	return Vector4i(
+			Math::snapped(x, p_step.x),
+			Math::snapped(y, p_step.y),
+			Math::snapped(z, p_step.z),
+			Math::snapped(w, p_step.w));
+}
+
+Vector4i Vector4i::snappedi(int32_t p_step) const {
+	return Vector4i(
+			Math::snapped(x, p_step),
+			Math::snapped(y, p_step),
+			Math::snapped(z, p_step),
+			Math::snapped(w, p_step));
+}
+
+Vector4i::operator String() const {
+	return "(" + itos(x) + ", " + itos(y) + ", " + itos(z) + ", " + itos(w) + ")";
+}
+
+Vector4i::operator Vector4() const {
+	return Vector4(x, y, z, w);
+}
+
+Vector4i::Vector4i(const Vector4 &p_vec4) {
+	x = (int32_t)p_vec4.x;
+	y = (int32_t)p_vec4.y;
+	z = (int32_t)p_vec4.z;
+	w = (int32_t)p_vec4.w;
+}
+
+static_assert(sizeof(Vector4i) == 4 * sizeof(int32_t));

core/math/vector4i.h

@@ -0,0 +1,366 @@
+/**************************************************************************/
+/*  vector4i.h                                                            */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "core/error/error_macros.h"
+#include "core/math/math_funcs.h"
+
+class String;
+struct Vector4;
+
+struct [[nodiscard]] Vector4i {
+	static const int AXIS_COUNT = 4;
+
+	enum Axis {
+		AXIS_X,
+		AXIS_Y,
+		AXIS_Z,
+		AXIS_W,
+	};
+
+	union {
+		// NOLINTBEGIN(modernize-use-default-member-init)
+		struct {
+			int32_t x;
+			int32_t y;
+			int32_t z;
+			int32_t w;
+		};
+
+		int32_t coord[4] = { 0 };
+		// NOLINTEND(modernize-use-default-member-init)
+	};
+
+	_FORCE_INLINE_ const int32_t &operator[](int p_axis) const {
+		DEV_ASSERT((unsigned int)p_axis < 4);
+		return coord[p_axis];
+	}
+
+	_FORCE_INLINE_ int32_t &operator[](int p_axis) {
+		DEV_ASSERT((unsigned int)p_axis < 4);
+		return coord[p_axis];
+	}
+
+	Vector4i::Axis min_axis_index() const;
+	Vector4i::Axis max_axis_index() const;
+
+	Vector4i min(const Vector4i &p_vector4i) const {
+		return Vector4i(MIN(x, p_vector4i.x), MIN(y, p_vector4i.y), MIN(z, p_vector4i.z), MIN(w, p_vector4i.w));
+	}
+
+	Vector4i mini(int32_t p_scalar) const {
+		return Vector4i(MIN(x, p_scalar), MIN(y, p_scalar), MIN(z, p_scalar), MIN(w, p_scalar));
+	}
+
+	Vector4i max(const Vector4i &p_vector4i) const {
+		return Vector4i(MAX(x, p_vector4i.x), MAX(y, p_vector4i.y), MAX(z, p_vector4i.z), MAX(w, p_vector4i.w));
+	}
+
+	Vector4i maxi(int32_t p_scalar) const {
+		return Vector4i(MAX(x, p_scalar), MAX(y, p_scalar), MAX(z, p_scalar), MAX(w, p_scalar));
+	}
+
+	_FORCE_INLINE_ int64_t length_squared() const;
+	_FORCE_INLINE_ double length() const;
+
+	_FORCE_INLINE_ void zero();
+
+	_FORCE_INLINE_ double distance_to(const Vector4i &p_to) const;
+	_FORCE_INLINE_ int64_t distance_squared_to(const Vector4i &p_to) const;
+
+	_FORCE_INLINE_ Vector4i abs() const;
+	_FORCE_INLINE_ Vector4i sign() const;
+	Vector4i clamp(const Vector4i &p_min, const Vector4i &p_max) const;
+	Vector4i clampi(int32_t p_min, int32_t p_max) const;
+	Vector4i snapped(const Vector4i &p_step) const;
+	Vector4i snappedi(int32_t p_step) const;
+
+	/* Operators */
+
+	constexpr Vector4i &operator+=(const Vector4i &p_v);
+	constexpr Vector4i operator+(const Vector4i &p_v) const;
+	constexpr Vector4i &operator-=(const Vector4i &p_v);
+	constexpr Vector4i operator-(const Vector4i &p_v) const;
+	constexpr Vector4i &operator*=(const Vector4i &p_v);
+	constexpr Vector4i operator*(const Vector4i &p_v) const;
+	constexpr Vector4i &operator/=(const Vector4i &p_v);
+	constexpr Vector4i operator/(const Vector4i &p_v) const;
+	constexpr Vector4i &operator%=(const Vector4i &p_v);
+	constexpr Vector4i operator%(const Vector4i &p_v) const;
+
+	constexpr Vector4i &operator*=(int32_t p_scalar);
+	constexpr Vector4i operator*(int32_t p_scalar) const;
+	constexpr Vector4i &operator/=(int32_t p_scalar);
+	constexpr Vector4i operator/(int32_t p_scalar) const;
+	constexpr Vector4i &operator%=(int32_t p_scalar);
+	constexpr Vector4i operator%(int32_t p_scalar) const;
+
+	constexpr Vector4i operator-() const;
+
+	constexpr bool operator==(const Vector4i &p_v) const;
+	constexpr bool operator!=(const Vector4i &p_v) const;
+	constexpr bool operator<(const Vector4i &p_v) const;
+	constexpr bool operator<=(const Vector4i &p_v) const;
+	constexpr bool operator>(const Vector4i &p_v) const;
+	constexpr bool operator>=(const Vector4i &p_v) const;
+
+	explicit operator String() const;
+	operator Vector4() const;
+
+	constexpr Vector4i() :
+			x(0), y(0), z(0), w(0) {}
+	Vector4i(const Vector4 &p_vec4);
+	constexpr Vector4i(int32_t p_x, int32_t p_y, int32_t p_z, int32_t p_w) :
+			x(p_x), y(p_y), z(p_z), w(p_w) {}
+};
+
+int64_t Vector4i::length_squared() const {
+	return x * (int64_t)x + y * (int64_t)y + z * (int64_t)z + w * (int64_t)w;
+}
+
+double Vector4i::length() const {
+	return Math::sqrt((double)length_squared());
+}
+
+double Vector4i::distance_to(const Vector4i &p_to) const {
+	return (p_to - *this).length();
+}
+
+int64_t Vector4i::distance_squared_to(const Vector4i &p_to) const {
+	return (p_to - *this).length_squared();
+}
+
+Vector4i Vector4i::abs() const {
+	return Vector4i(Math::abs(x), Math::abs(y), Math::abs(z), Math::abs(w));
+}
+
+Vector4i Vector4i::sign() const {
+	return Vector4i(SIGN(x), SIGN(y), SIGN(z), SIGN(w));
+}
+
+/* Operators */
+
+constexpr Vector4i &Vector4i::operator+=(const Vector4i &p_v) {
+	x += p_v.x;
+	y += p_v.y;
+	z += p_v.z;
+	w += p_v.w;
+	return *this;
+}
+
+constexpr Vector4i Vector4i::operator+(const Vector4i &p_v) const {
+	return Vector4i(x + p_v.x, y + p_v.y, z + p_v.z, w + p_v.w);
+}
+
+constexpr Vector4i &Vector4i::operator-=(const Vector4i &p_v) {
+	x -= p_v.x;
+	y -= p_v.y;
+	z -= p_v.z;
+	w -= p_v.w;
+	return *this;
+}
+
+constexpr Vector4i Vector4i::operator-(const Vector4i &p_v) const {
+	return Vector4i(x - p_v.x, y - p_v.y, z - p_v.z, w - p_v.w);
+}
+
+constexpr Vector4i &Vector4i::operator*=(const Vector4i &p_v) {
+	x *= p_v.x;
+	y *= p_v.y;
+	z *= p_v.z;
+	w *= p_v.w;
+	return *this;
+}
+
+constexpr Vector4i Vector4i::operator*(const Vector4i &p_v) const {
+	return Vector4i(x * p_v.x, y * p_v.y, z * p_v.z, w * p_v.w);
+}
+
+constexpr Vector4i &Vector4i::operator/=(const Vector4i &p_v) {
+	x /= p_v.x;
+	y /= p_v.y;
+	z /= p_v.z;
+	w /= p_v.w;
+	return *this;
+}
+
+constexpr Vector4i Vector4i::operator/(const Vector4i &p_v) const {
+	return Vector4i(x / p_v.x, y / p_v.y, z / p_v.z, w / p_v.w);
+}
+
+constexpr Vector4i &Vector4i::operator%=(const Vector4i &p_v) {
+	x %= p_v.x;
+	y %= p_v.y;
+	z %= p_v.z;
+	w %= p_v.w;
+	return *this;
+}
+
+constexpr Vector4i Vector4i::operator%(const Vector4i &p_v) const {
+	return Vector4i(x % p_v.x, y % p_v.y, z % p_v.z, w % p_v.w);
+}
+
+constexpr Vector4i &Vector4i::operator*=(int32_t p_scalar) {
+	x *= p_scalar;
+	y *= p_scalar;
+	z *= p_scalar;
+	w *= p_scalar;
+	return *this;
+}
+
+constexpr Vector4i Vector4i::operator*(int32_t p_scalar) const {
+	return Vector4i(x * p_scalar, y * p_scalar, z * p_scalar, w * p_scalar);
+}
+
+// Multiplication operators required to workaround issues with LLVM using implicit conversion.
+
+constexpr Vector4i operator*(int32_t p_scalar, const Vector4i &p_vector) {
+	return p_vector * p_scalar;
+}
+
+constexpr Vector4i operator*(int64_t p_scalar, const Vector4i &p_vector) {
+	return p_vector * p_scalar;
+}
+
+constexpr Vector4i operator*(float p_scalar, const Vector4i &p_vector) {
+	return p_vector * p_scalar;
+}
+
+constexpr Vector4i operator*(double p_scalar, const Vector4i &p_vector) {
+	return p_vector * p_scalar;
+}
+
+constexpr Vector4i &Vector4i::operator/=(int32_t p_scalar) {
+	x /= p_scalar;
+	y /= p_scalar;
+	z /= p_scalar;
+	w /= p_scalar;
+	return *this;
+}
+
+constexpr Vector4i Vector4i::operator/(int32_t p_scalar) const {
+	return Vector4i(x / p_scalar, y / p_scalar, z / p_scalar, w / p_scalar);
+}
+
+constexpr Vector4i &Vector4i::operator%=(int32_t p_scalar) {
+	x %= p_scalar;
+	y %= p_scalar;
+	z %= p_scalar;
+	w %= p_scalar;
+	return *this;
+}
+
+constexpr Vector4i Vector4i::operator%(int32_t p_scalar) const {
+	return Vector4i(x % p_scalar, y % p_scalar, z % p_scalar, w % p_scalar);
+}
+
+constexpr Vector4i Vector4i::operator-() const {
+	return Vector4i(-x, -y, -z, -w);
+}
+
+constexpr bool Vector4i::operator==(const Vector4i &p_v) const {
+	return (x == p_v.x && y == p_v.y && z == p_v.z && w == p_v.w);
+}
+
+constexpr bool Vector4i::operator!=(const Vector4i &p_v) const {
+	return (x != p_v.x || y != p_v.y || z != p_v.z || w != p_v.w);
+}
+
+constexpr bool Vector4i::operator<(const Vector4i &p_v) const {
+	if (x == p_v.x) {
+		if (y == p_v.y) {
+			if (z == p_v.z) {
+				return w < p_v.w;
+			} else {
+				return z < p_v.z;
+			}
+		} else {
+			return y < p_v.y;
+		}
+	} else {
+		return x < p_v.x;
+	}
+}
+
+constexpr bool Vector4i::operator>(const Vector4i &p_v) const {
+	if (x == p_v.x) {
+		if (y == p_v.y) {
+			if (z == p_v.z) {
+				return w > p_v.w;
+			} else {
+				return z > p_v.z;
+			}
+		} else {
+			return y > p_v.y;
+		}
+	} else {
+		return x > p_v.x;
+	}
+}
+
+constexpr bool Vector4i::operator<=(const Vector4i &p_v) const {
+	if (x == p_v.x) {
+		if (y == p_v.y) {
+			if (z == p_v.z) {
+				return w <= p_v.w;
+			} else {
+				return z < p_v.z;
+			}
+		} else {
+			return y < p_v.y;
+		}
+	} else {
+		return x < p_v.x;
+	}
+}
+
+constexpr bool Vector4i::operator>=(const Vector4i &p_v) const {
+	if (x == p_v.x) {
+		if (y == p_v.y) {
+			if (z == p_v.z) {
+				return w >= p_v.w;
+			} else {
+				return z > p_v.z;
+			}
+		} else {
+			return y > p_v.y;
+		}
+	} else {
+		return x > p_v.x;
+	}
+}
+
+void Vector4i::zero() {
+	x = y = z = w = 0;
+}
+
+template <>
+struct is_zero_constructible<Vector4i> : std::true_type {};