summaryrefslogtreecommitdiff
path: root/core/math
diff options
context:
space:
mode:
Diffstat (limited to 'core/math')
-rw-r--r--core/math/basis.cpp12
-rw-r--r--core/math/basis.h24
-rw-r--r--core/math/expression.cpp40
-rw-r--r--core/math/expression.h4
-rw-r--r--core/math/math_funcs.h21
-rw-r--r--core/math/octree.h1271
-rw-r--r--core/math/vector2.cpp7
-rw-r--r--core/math/vector2.h24
-rw-r--r--core/math/vector3.cpp8
-rw-r--r--core/math/vector3.h25
10 files changed, 110 insertions, 1326 deletions
diff --git a/core/math/basis.cpp b/core/math/basis.cpp
index 65353d8118..ce5e9aa9b3 100644
--- a/core/math/basis.cpp
+++ b/core/math/basis.cpp
@@ -365,12 +365,12 @@ Basis Basis::rotated_local(const Vector3 &p_axis, real_t p_angle) const {
return (*this) * Basis(p_axis, p_angle);
}
-Basis Basis::rotated(const Vector3 &p_euler) const {
- return Basis(p_euler) * (*this);
+Basis Basis::rotated(const Vector3 &p_euler, EulerOrder p_order) const {
+ return Basis::from_euler(p_euler, p_order) * (*this);
}
-void Basis::rotate(const Vector3 &p_euler) {
- *this = rotated(p_euler);
+void Basis::rotate(const Vector3 &p_euler, EulerOrder p_order) {
+ *this = rotated(p_euler, p_order);
}
Basis Basis::rotated(const Quaternion &p_quaternion) const {
@@ -935,9 +935,9 @@ void Basis::set_axis_angle_scale(const Vector3 &p_axis, real_t p_angle, const Ve
rotate(p_axis, p_angle);
}
-void Basis::set_euler_scale(const Vector3 &p_euler, const Vector3 &p_scale) {
+void Basis::set_euler_scale(const Vector3 &p_euler, const Vector3 &p_scale, EulerOrder p_order) {
_set_diagonal(p_scale);
- rotate(p_euler);
+ rotate(p_euler, p_order);
}
void Basis::set_quaternion_scale(const Quaternion &p_quaternion, const Vector3 &p_scale) {
diff --git a/core/math/basis.h b/core/math/basis.h
index 9cce22510b..4be325cdd2 100644
--- a/core/math/basis.h
+++ b/core/math/basis.h
@@ -56,6 +56,15 @@ struct _NO_DISCARD_ Basis {
_FORCE_INLINE_ real_t determinant() const;
+ enum EulerOrder {
+ EULER_ORDER_XYZ,
+ EULER_ORDER_XZY,
+ EULER_ORDER_YXZ,
+ EULER_ORDER_YZX,
+ EULER_ORDER_ZXY,
+ EULER_ORDER_ZYX
+ };
+
void from_z(const Vector3 &p_z);
void rotate(const Vector3 &p_axis, real_t p_angle);
@@ -64,21 +73,12 @@ struct _NO_DISCARD_ Basis {
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);
- Basis rotated(const Vector3 &p_euler) const;
+ void rotate(const Vector3 &p_euler, EulerOrder p_order = EULER_ORDER_YXZ);
+ Basis rotated(const Vector3 &p_euler, EulerOrder p_order = EULER_ORDER_YXZ) const;
void rotate(const Quaternion &p_quaternion);
Basis rotated(const Quaternion &p_quaternion) const;
- enum EulerOrder {
- EULER_ORDER_XYZ,
- EULER_ORDER_XZY,
- EULER_ORDER_YXZ,
- EULER_ORDER_YZX,
- EULER_ORDER_ZXY,
- EULER_ORDER_ZYX
- };
-
Vector3 get_euler_normalized(EulerOrder p_order = EULER_ORDER_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;
@@ -119,7 +119,7 @@ struct _NO_DISCARD_ Basis {
Vector3 get_scale_local() 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);
+ void set_euler_scale(const Vector3 &p_euler, const Vector3 &p_scale, EulerOrder p_order = EULER_ORDER_YXZ);
void set_quaternion_scale(const Quaternion &p_quaternion, const Vector3 &p_scale);
// transposed dot products
diff --git a/core/math/expression.cpp b/core/math/expression.cpp
index 5a90f68b66..e230b69dc9 100644
--- a/core/math/expression.cpp
+++ b/core/math/expression.cpp
@@ -891,7 +891,7 @@ Expression::ENode *Expression::_parse_expression() {
case TK_PERIOD: {
//named indexing or function call
_get_token(tk);
- if (tk.type != TK_IDENTIFIER) {
+ if (tk.type != TK_IDENTIFIER && tk.type != TK_BUILTIN_FUNC) {
_set_error("Expected identifier after '.'");
return nullptr;
}
@@ -1240,7 +1240,7 @@ bool Expression::_compile_expression() {
return false;
}
-bool Expression::_execute(const Array &p_inputs, Object *p_instance, Expression::ENode *p_node, Variant &r_ret, String &r_error_str) {
+bool Expression::_execute(const Array &p_inputs, Object *p_instance, Expression::ENode *p_node, Variant &r_ret, bool p_const_calls_only, String &r_error_str) {
switch (p_node->type) {
case Expression::ENode::TYPE_INPUT: {
const Expression::InputNode *in = static_cast<const Expression::InputNode *>(p_node);
@@ -1266,7 +1266,7 @@ bool Expression::_execute(const Array &p_inputs, Object *p_instance, Expression:
const Expression::OperatorNode *op = static_cast<const Expression::OperatorNode *>(p_node);
Variant a;
- bool ret = _execute(p_inputs, p_instance, op->nodes[0], a, r_error_str);
+ bool ret = _execute(p_inputs, p_instance, op->nodes[0], a, p_const_calls_only, r_error_str);
if (ret) {
return true;
}
@@ -1274,7 +1274,7 @@ bool Expression::_execute(const Array &p_inputs, Object *p_instance, Expression:
Variant b;
if (op->nodes[1]) {
- ret = _execute(p_inputs, p_instance, op->nodes[1], b, r_error_str);
+ ret = _execute(p_inputs, p_instance, op->nodes[1], b, p_const_calls_only, r_error_str);
if (ret) {
return true;
}
@@ -1292,14 +1292,14 @@ bool Expression::_execute(const Array &p_inputs, Object *p_instance, Expression:
const Expression::IndexNode *index = static_cast<const Expression::IndexNode *>(p_node);
Variant base;
- bool ret = _execute(p_inputs, p_instance, index->base, base, r_error_str);
+ bool ret = _execute(p_inputs, p_instance, index->base, base, p_const_calls_only, r_error_str);
if (ret) {
return true;
}
Variant idx;
- ret = _execute(p_inputs, p_instance, index->index, idx, r_error_str);
+ ret = _execute(p_inputs, p_instance, index->index, idx, p_const_calls_only, r_error_str);
if (ret) {
return true;
}
@@ -1316,7 +1316,7 @@ bool Expression::_execute(const Array &p_inputs, Object *p_instance, Expression:
const Expression::NamedIndexNode *index = static_cast<const Expression::NamedIndexNode *>(p_node);
Variant base;
- bool ret = _execute(p_inputs, p_instance, index->base, base, r_error_str);
+ bool ret = _execute(p_inputs, p_instance, index->base, base, p_const_calls_only, r_error_str);
if (ret) {
return true;
}
@@ -1336,7 +1336,7 @@ bool Expression::_execute(const Array &p_inputs, Object *p_instance, Expression:
arr.resize(array->array.size());
for (int i = 0; i < array->array.size(); i++) {
Variant value;
- bool ret = _execute(p_inputs, p_instance, array->array[i], value, r_error_str);
+ bool ret = _execute(p_inputs, p_instance, array->array[i], value, p_const_calls_only, r_error_str);
if (ret) {
return true;
@@ -1353,14 +1353,14 @@ bool Expression::_execute(const Array &p_inputs, Object *p_instance, Expression:
Dictionary d;
for (int i = 0; i < dictionary->dict.size(); i += 2) {
Variant key;
- bool ret = _execute(p_inputs, p_instance, dictionary->dict[i + 0], key, r_error_str);
+ bool ret = _execute(p_inputs, p_instance, dictionary->dict[i + 0], key, p_const_calls_only, r_error_str);
if (ret) {
return true;
}
Variant value;
- ret = _execute(p_inputs, p_instance, dictionary->dict[i + 1], value, r_error_str);
+ ret = _execute(p_inputs, p_instance, dictionary->dict[i + 1], value, p_const_calls_only, r_error_str);
if (ret) {
return true;
}
@@ -1380,7 +1380,7 @@ bool Expression::_execute(const Array &p_inputs, Object *p_instance, Expression:
for (int i = 0; i < constructor->arguments.size(); i++) {
Variant value;
- bool ret = _execute(p_inputs, p_instance, constructor->arguments[i], value, r_error_str);
+ bool ret = _execute(p_inputs, p_instance, constructor->arguments[i], value, p_const_calls_only, r_error_str);
if (ret) {
return true;
@@ -1408,7 +1408,7 @@ bool Expression::_execute(const Array &p_inputs, Object *p_instance, Expression:
for (int i = 0; i < bifunc->arguments.size(); i++) {
Variant value;
- bool ret = _execute(p_inputs, p_instance, bifunc->arguments[i], value, r_error_str);
+ bool ret = _execute(p_inputs, p_instance, bifunc->arguments[i], value, p_const_calls_only, r_error_str);
if (ret) {
return true;
}
@@ -1429,7 +1429,7 @@ bool Expression::_execute(const Array &p_inputs, Object *p_instance, Expression:
const Expression::CallNode *call = static_cast<const Expression::CallNode *>(p_node);
Variant base;
- bool ret = _execute(p_inputs, p_instance, call->base, base, r_error_str);
+ bool ret = _execute(p_inputs, p_instance, call->base, base, p_const_calls_only, r_error_str);
if (ret) {
return true;
@@ -1442,7 +1442,7 @@ bool Expression::_execute(const Array &p_inputs, Object *p_instance, Expression:
for (int i = 0; i < call->arguments.size(); i++) {
Variant value;
- ret = _execute(p_inputs, p_instance, call->arguments[i], value, r_error_str);
+ ret = _execute(p_inputs, p_instance, call->arguments[i], value, p_const_calls_only, r_error_str);
if (ret) {
return true;
@@ -1452,7 +1452,11 @@ bool Expression::_execute(const Array &p_inputs, Object *p_instance, Expression:
}
Callable::CallError ce;
- base.callp(call->method, (const Variant **)argp.ptr(), argp.size(), r_ret, ce);
+ if (p_const_calls_only) {
+ base.call_const(call->method, (const Variant **)argp.ptr(), argp.size(), r_ret, ce);
+ } else {
+ base.callp(call->method, (const Variant **)argp.ptr(), argp.size(), r_ret, ce);
+ }
if (ce.error != Callable::CallError::CALL_OK) {
r_error_str = vformat(RTR("On call to '%s':"), String(call->method));
@@ -1491,13 +1495,13 @@ Error Expression::parse(const String &p_expression, const Vector<String> &p_inpu
return OK;
}
-Variant Expression::execute(Array p_inputs, Object *p_base, bool p_show_error) {
+Variant Expression::execute(Array p_inputs, Object *p_base, bool p_show_error, bool p_const_calls_only) {
ERR_FAIL_COND_V_MSG(error_set, Variant(), "There was previously a parse error: " + error_str + ".");
execution_error = false;
Variant output;
String error_txt;
- bool err = _execute(p_inputs, p_base, root, output, error_txt);
+ bool err = _execute(p_inputs, p_base, root, output, p_const_calls_only, error_txt);
if (err) {
execution_error = true;
error_str = error_txt;
@@ -1517,7 +1521,7 @@ String Expression::get_error_text() const {
void Expression::_bind_methods() {
ClassDB::bind_method(D_METHOD("parse", "expression", "input_names"), &Expression::parse, DEFVAL(Vector<String>()));
- ClassDB::bind_method(D_METHOD("execute", "inputs", "base_instance", "show_error"), &Expression::execute, DEFVAL(Array()), DEFVAL(Variant()), DEFVAL(true));
+ ClassDB::bind_method(D_METHOD("execute", "inputs", "base_instance", "show_error", "const_calls_only"), &Expression::execute, DEFVAL(Array()), DEFVAL(Variant()), DEFVAL(true), DEFVAL(false));
ClassDB::bind_method(D_METHOD("has_execute_failed"), &Expression::has_execute_failed);
ClassDB::bind_method(D_METHOD("get_error_text"), &Expression::get_error_text);
}
diff --git a/core/math/expression.h b/core/math/expression.h
index 6ea3c1611f..2d58915996 100644
--- a/core/math/expression.h
+++ b/core/math/expression.h
@@ -257,14 +257,14 @@ private:
Vector<String> input_names;
bool execution_error = false;
- bool _execute(const Array &p_inputs, Object *p_instance, Expression::ENode *p_node, Variant &r_ret, String &r_error_str);
+ 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(Array p_inputs = Array(), Object *p_base = nullptr, bool p_show_error = true);
+ Variant execute(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;
diff --git a/core/math/math_funcs.h b/core/math/math_funcs.h
index c8a55341aa..53deb9bd42 100644
--- a/core/math/math_funcs.h
+++ b/core/math/math_funcs.h
@@ -253,6 +253,27 @@ public:
(-p_pre + 3.0f * p_from - 3.0f * p_to + p_post) * (p_weight * p_weight * p_weight));
}
+ static _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;
+ }
+ static _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;
+ }
+
static _ALWAYS_INLINE_ double lerp_angle(double p_from, double p_to, double p_weight) {
double difference = fmod(p_to - p_from, Math_TAU);
double distance = fmod(2.0 * difference, Math_TAU) - difference;
diff --git a/core/math/octree.h b/core/math/octree.h
deleted file mode 100644
index 8dd103f109..0000000000
--- a/core/math/octree.h
+++ /dev/null
@@ -1,1271 +0,0 @@
-/*************************************************************************/
-/* octree.h */
-/*************************************************************************/
-/* This file is part of: */
-/* GODOT ENGINE */
-/* https://godotengine.org */
-/*************************************************************************/
-/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */
-/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md). */
-/* */
-/* 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 OCTREE_H
-#define OCTREE_H
-
-#include "core/math/aabb.h"
-#include "core/math/geometry_3d.h"
-#include "core/math/vector3.h"
-#include "core/string/print_string.h"
-#include "core/templates/list.h"
-#include "core/templates/rb_map.h"
-#include "core/variant/variant.h"
-
-typedef uint32_t OctreeElementID;
-
-#define OCTREE_ELEMENT_INVALID_ID 0
-#define OCTREE_SIZE_LIMIT 1e15
-
-template <class T, bool use_pairs = false, class AL = DefaultAllocator>
-class Octree {
-public:
- typedef void *(*PairCallback)(void *, OctreeElementID, T *, int, OctreeElementID, T *, int);
- typedef void (*UnpairCallback)(void *, OctreeElementID, T *, int, OctreeElementID, T *, int, void *);
-
-private:
- enum {
- NEG = 0,
- POS = 1,
- };
-
- enum {
- OCTANT_NX_NY_NZ,
- OCTANT_PX_NY_NZ,
- OCTANT_NX_PY_NZ,
- OCTANT_PX_PY_NZ,
- OCTANT_NX_NY_PZ,
- OCTANT_PX_NY_PZ,
- OCTANT_NX_PY_PZ,
- OCTANT_PX_PY_PZ
- };
-
- struct PairKey {
- union {
- struct {
- OctreeElementID A;
- OctreeElementID B;
- };
- uint64_t key;
- };
-
- _FORCE_INLINE_ bool operator<(const PairKey &p_pair) const {
- return key < p_pair.key;
- }
-
- _FORCE_INLINE_ PairKey(OctreeElementID p_A, OctreeElementID p_B) {
- if (p_A < p_B) {
- A = p_A;
- B = p_B;
- } else {
- B = p_A;
- A = p_B;
- }
- }
-
- _FORCE_INLINE_ PairKey() {}
- };
-
- struct Element;
-
- struct Octant {
- // cached for FAST plane check
- AABB aabb;
-
- uint64_t last_pass = 0;
- Octant *parent = nullptr;
- Octant *children[8] = { nullptr };
-
- int children_count = 0; // cache for amount of children (fast check for removal)
- int parent_index = -1; // cache for parent index (fast check for removal)
-
- List<Element *, AL> pairable_elements;
- List<Element *, AL> elements;
-
- Octant() {}
- ~Octant() {}
- };
-
- struct PairData;
-
- struct Element {
- Octree *octree = nullptr;
-
- T *userdata = nullptr;
- int subindex = 0;
- bool pairable = false;
- uint32_t pairable_mask = 0;
- uint32_t pairable_type = 0;
-
- uint64_t last_pass = 0;
- OctreeElementID _id = 0;
- Octant *common_parent = nullptr;
-
- AABB aabb;
- AABB container_aabb;
-
- List<PairData *, AL> pair_list;
-
- struct OctantOwner {
- Octant *octant = nullptr;
- typename List<Element *, AL>::Element *E;
- }; // an element can be in max 8 octants
-
- List<OctantOwner, AL> octant_owners;
-
- Element() {}
- };
-
- struct PairData {
- int refcount;
- bool intersect;
- Element *A, *B;
- void *ud = nullptr;
- typename List<PairData *, AL>::Element *eA, *eB;
- };
-
- typedef HashMap<OctreeElementID, Element, Comparator<OctreeElementID>, AL> ElementMap;
- typedef HashMap<PairKey, PairData, Comparator<PairKey>, AL> PairMap;
- ElementMap element_map;
- PairMap pair_map;
-
- PairCallback pair_callback = nullptr;
- UnpairCallback unpair_callback = nullptr;
- void *pair_callback_userdata = nullptr;
- void *unpair_callback_userdata = nullptr;
-
- OctreeElementID last_element_id = 1;
- uint64_t pass = 1;
-
- real_t unit_size = 1.0;
- Octant *root = nullptr;
- int octant_count = 0;
- int pair_count = 0;
-
- _FORCE_INLINE_ void _pair_check(PairData *p_pair) {
- bool intersect = p_pair->A->aabb.intersects_inclusive(p_pair->B->aabb);
-
- if (intersect != p_pair->intersect) {
- if (intersect) {
- if (pair_callback) {
- p_pair->ud = pair_callback(pair_callback_userdata, p_pair->A->_id, p_pair->A->userdata, p_pair->A->subindex, p_pair->B->_id, p_pair->B->userdata, p_pair->B->subindex);
- }
- pair_count++;
- } else {
- if (unpair_callback) {
- unpair_callback(pair_callback_userdata, p_pair->A->_id, p_pair->A->userdata, p_pair->A->subindex, p_pair->B->_id, p_pair->B->userdata, p_pair->B->subindex, p_pair->ud);
- }
- pair_count--;
- }
-
- p_pair->intersect = intersect;
- }
- }
-
- _FORCE_INLINE_ void _pair_reference(Element *p_A, Element *p_B) {
- if (p_A == p_B || (p_A->userdata == p_B->userdata && p_A->userdata)) {
- return;
- }
-
- if (!(p_A->pairable_type & p_B->pairable_mask) &&
- !(p_B->pairable_type & p_A->pairable_mask)) {
- return; // none can pair with none
- }
-
- PairKey key(p_A->_id, p_B->_id);
- typename PairMap::Element *E = pair_map.find(key);
-
- if (!E) {
- PairData pdata;
- pdata.refcount = 1;
- pdata.A = p_A;
- pdata.B = p_B;
- pdata.intersect = false;
- E = pair_map.insert(key, pdata);
- E->get().eA = p_A->pair_list.push_back(&E->get());
- E->get().eB = p_B->pair_list.push_back(&E->get());
- } else {
- E->get().refcount++;
- }
- }
-
- _FORCE_INLINE_ void _pair_unreference(Element *p_A, Element *p_B) {
- if (p_A == p_B) {
- return;
- }
-
- PairKey key(p_A->_id, p_B->_id);
- typename PairMap::Element *E = pair_map.find(key);
- if (!E) {
- return; // no pair
- }
-
- E->get().refcount--;
-
- if (E->get().refcount == 0) {
- // bye pair
-
- if (E->get().intersect) {
- if (unpair_callback) {
- unpair_callback(pair_callback_userdata, p_A->_id, p_A->userdata, p_A->subindex, p_B->_id, p_B->userdata, p_B->subindex, E->get().ud);
- }
-
- pair_count--;
- }
-
- if (p_A == E->get().B) {
- //may be reaching inverted
- SWAP(p_A, p_B);
- }
-
- p_A->pair_list.erase(E->get().eA);
- p_B->pair_list.erase(E->get().eB);
- pair_map.erase(E);
- }
- }
-
- _FORCE_INLINE_ void _element_check_pairs(Element *p_element) {
- typename List<PairData *, AL>::Element *E = p_element->pair_list.front();
- while (E) {
- _pair_check(E->get());
- E = E->next();
- }
- }
-
- _FORCE_INLINE_ void _optimize() {
- while (root && root->children_count < 2 && !root->elements.size() && !(use_pairs && root->pairable_elements.size())) {
- Octant *new_root = nullptr;
- if (root->children_count == 1) {
- for (int i = 0; i < 8; i++) {
- if (root->children[i]) {
- new_root = root->children[i];
- root->children[i] = nullptr;
- break;
- }
- }
- ERR_FAIL_COND(!new_root);
- new_root->parent = nullptr;
- new_root->parent_index = -1;
- }
-
- memdelete_allocator<Octant, AL>(root);
- octant_count--;
- root = new_root;
- }
- }
-
- void _insert_element(Element *p_element, Octant *p_octant);
- void _ensure_valid_root(const AABB &p_aabb);
- bool _remove_element_from_octant(Element *p_element, Octant *p_octant, Octant *p_limit = nullptr);
- void _remove_element(Element *p_element);
- void _pair_element(Element *p_element, Octant *p_octant);
- void _unpair_element(Element *p_element, Octant *p_octant);
-
- struct _CullConvexData {
- const Plane *planes;
- int plane_count;
- const Vector3 *points;
- int point_count;
- T **result_array;
- int *result_idx = nullptr;
- int result_max;
- uint32_t mask;
- };
-
- void _cull_convex(Octant *p_octant, _CullConvexData *p_cull);
- void _cull_aabb(Octant *p_octant, const AABB &p_aabb, T **p_result_array, int *p_result_idx, int p_result_max, int *p_subindex_array, uint32_t p_mask);
- void _cull_segment(Octant *p_octant, const Vector3 &p_from, const Vector3 &p_to, T **p_result_array, int *p_result_idx, int p_result_max, int *p_subindex_array, uint32_t p_mask);
- void _cull_point(Octant *p_octant, const Vector3 &p_point, T **p_result_array, int *p_result_idx, int p_result_max, int *p_subindex_array, uint32_t p_mask);
-
- void _remove_tree(Octant *p_octant) {
- if (!p_octant) {
- return;
- }
-
- for (int i = 0; i < 8; i++) {
- if (p_octant->children[i]) {
- _remove_tree(p_octant->children[i]);
- }
- }
-
- memdelete_allocator<Octant, AL>(p_octant);
- }
-
-public:
- OctreeElementID create(T *p_userdata, const AABB &p_aabb = AABB(), int p_subindex = 0, bool p_pairable = false, uint32_t p_pairable_type = 0, uint32_t pairable_mask = 1);
- void move(OctreeElementID p_id, const AABB &p_aabb);
- void set_pairable(OctreeElementID p_id, bool p_pairable = false, uint32_t p_pairable_type = 0, uint32_t pairable_mask = 1);
- void erase(OctreeElementID p_id);
-
- bool is_pairable(OctreeElementID p_id) const;
- T *get(OctreeElementID p_id) const;
- int get_subindex(OctreeElementID p_id) const;
-
- int cull_convex(const Vector<Plane> &p_convex, T **p_result_array, int p_result_max, uint32_t p_mask = 0xFFFFFFFF);
- int cull_aabb(const AABB &p_aabb, T **p_result_array, int p_result_max, int *p_subindex_array = nullptr, uint32_t p_mask = 0xFFFFFFFF);
- int cull_segment(const Vector3 &p_from, const Vector3 &p_to, T **p_result_array, int p_result_max, int *p_subindex_array = nullptr, uint32_t p_mask = 0xFFFFFFFF);
-
- int cull_point(const Vector3 &p_point, T **p_result_array, int p_result_max, int *p_subindex_array = nullptr, uint32_t p_mask = 0xFFFFFFFF);
-
- void set_pair_callback(PairCallback p_callback, void *p_userdata);
- void set_unpair_callback(UnpairCallback p_callback, void *p_userdata);
-
- int get_octant_count() const { return octant_count; }
- int get_pair_count() const { return pair_count; }
- Octree(real_t p_unit_size = 1.0);
- ~Octree() { _remove_tree(root); }
-};
-
-/* PRIVATE FUNCTIONS */
-
-template <class T, bool use_pairs, class AL>
-T *Octree<T, use_pairs, AL>::get(OctreeElementID p_id) const {
- const typename ElementMap::Element *E = element_map.find(p_id);
- ERR_FAIL_COND_V(!E, nullptr);
- return E->get().userdata;
-}
-
-template <class T, bool use_pairs, class AL>
-bool Octree<T, use_pairs, AL>::is_pairable(OctreeElementID p_id) const {
- const typename ElementMap::Element *E = element_map.find(p_id);
- ERR_FAIL_COND_V(!E, false);
- return E->get().pairable;
-}
-
-template <class T, bool use_pairs, class AL>
-int Octree<T, use_pairs, AL>::get_subindex(OctreeElementID p_id) const {
- const typename ElementMap::Element *E = element_map.find(p_id);
- ERR_FAIL_COND_V(!E, -1);
- return E->get().subindex;
-}
-
-#define OCTREE_DIVISOR 4
-
-template <class T, bool use_pairs, class AL>
-void Octree<T, use_pairs, AL>::_insert_element(Element *p_element, Octant *p_octant) {
- real_t element_size = p_element->aabb.get_longest_axis_size() * 1.01; // avoid precision issues
-
- if (p_octant->aabb.size.x / OCTREE_DIVISOR < element_size) {
- //if (p_octant->aabb.size.x*0.5 < element_size) {
- /* at smallest possible size for the element */
- typename Element::OctantOwner owner;
- owner.octant = p_octant;
-
- if (use_pairs && p_element->pairable) {
- p_octant->pairable_elements.push_back(p_element);
- owner.E = p_octant->pairable_elements.back();
- } else {
- p_octant->elements.push_back(p_element);
- owner.E = p_octant->elements.back();
- }
-
- p_element->octant_owners.push_back(owner);
-
- if (p_element->common_parent == nullptr) {
- p_element->common_parent = p_octant;
- p_element->container_aabb = p_octant->aabb;
- } else {
- p_element->container_aabb.merge_with(p_octant->aabb);
- }
-
- if (use_pairs && p_octant->children_count > 0) {
- pass++; //elements below this only get ONE reference added
-
- for (int i = 0; i < 8; i++) {
- if (p_octant->children[i]) {
- _pair_element(p_element, p_octant->children[i]);
- }
- }
- }
- } else {
- /* not big enough, send it to subitems */
- int splits = 0;
- bool candidate = p_element->common_parent == nullptr;
-
- for (int i = 0; i < 8; i++) {
- if (p_octant->children[i]) {
- /* element exists, go straight to it */
- if (p_octant->children[i]->aabb.intersects_inclusive(p_element->aabb)) {
- _insert_element(p_element, p_octant->children[i]);
- splits++;
- }
- } else {
- /* check against AABB where child should be */
-
- AABB aabb = p_octant->aabb;
- aabb.size *= 0.5;
-
- if (i & 1) {
- aabb.position.x += aabb.size.x;
- }
- if (i & 2) {
- aabb.position.y += aabb.size.y;
- }
- if (i & 4) {
- aabb.position.z += aabb.size.z;
- }
-
- if (aabb.intersects_inclusive(p_element->aabb)) {
- /* if actually intersects, create the child */
-
- Octant *child = memnew_allocator(Octant, AL);
- p_octant->children[i] = child;
- child->parent = p_octant;
- child->parent_index = i;
-
- child->aabb = aabb;
-
- p_octant->children_count++;
-
- _insert_element(p_element, child);
- octant_count++;
- splits++;
- }
- }
- }
-
- if (candidate && splits > 1) {
- p_element->common_parent = p_octant;
- }
- }
-
- if (use_pairs) {
- typename List<Element *, AL>::Element *E = p_octant->pairable_elements.front();
-
- while (E) {
- _pair_reference(p_element, E->get());
- E = E->next();
- }
-
- if (p_element->pairable) {
- // and always test non-pairable if element is pairable
- E = p_octant->elements.front();
- while (E) {
- _pair_reference(p_element, E->get());
- E = E->next();
- }
- }
- }
-}
-
-template <class T, bool use_pairs, class AL>
-void Octree<T, use_pairs, AL>::_ensure_valid_root(const AABB &p_aabb) {
- if (!root) {
- // octre is empty
-
- AABB base(Vector3(), Vector3(1.0, 1.0, 1.0) * unit_size);
-
- while (!base.encloses(p_aabb)) {
- if (ABS(base.position.x + base.size.x) <= ABS(base.position.x)) {
- /* grow towards positive */
- base.size *= 2.0;
- } else {
- base.position -= base.size;
- base.size *= 2.0;
- }
- }
-
- root = memnew_allocator(Octant, AL);
-
- root->parent = nullptr;
- root->parent_index = -1;
- root->aabb = base;
-
- octant_count++;
-
- } else {
- AABB base = root->aabb;
-
- while (!base.encloses(p_aabb)) {
- ERR_FAIL_COND_MSG(base.size.x > OCTREE_SIZE_LIMIT, "Octree upper size limit reached, does the AABB supplied contain NAN?");
-
- Octant *gp = memnew_allocator(Octant, AL);
- octant_count++;
- root->parent = gp;
-
- if (ABS(base.position.x + base.size.x) <= ABS(base.position.x)) {
- /* grow towards positive */
- base.size *= 2.0;
- gp->aabb = base;
- gp->children[0] = root;
- root->parent_index = 0;
- } else {
- base.position -= base.size;
- base.size *= 2.0;
- gp->aabb = base;
- gp->children[(1 << 0) | (1 << 1) | (1 << 2)] = root; // add at all-positive
- root->parent_index = (1 << 0) | (1 << 1) | (1 << 2);
- }
-
- gp->children_count = 1;
- root = gp;
- }
- }
-}
-
-template <class T, bool use_pairs, class AL>
-bool Octree<T, use_pairs, AL>::_remove_element_from_octant(Element *p_element, Octant *p_octant, Octant *p_limit) {
- bool octant_removed = false;
-
- while (true) {
- // check all exit conditions
-
- if (p_octant == p_limit) { // reached limit, nothing to erase, exit
- return octant_removed;
- }
-
- bool unpaired = false;
-
- if (use_pairs && p_octant->last_pass != pass) {
- // check whether we should unpair stuff
- // always test pairable
- typename List<Element *, AL>::Element *E = p_octant->pairable_elements.front();
- while (E) {
- _pair_unreference(p_element, E->get());
- E = E->next();
- }
- if (p_element->pairable) {
- // and always test non-pairable if element is pairable
- E = p_octant->elements.front();
- while (E) {
- _pair_unreference(p_element, E->get());
- E = E->next();
- }
- }
- p_octant->last_pass = pass;
- unpaired = true;
- }
-
- bool removed = false;
-
- Octant *parent = p_octant->parent;
-
- if (p_octant->children_count == 0 && p_octant->elements.is_empty() && p_octant->pairable_elements.is_empty()) {
- // erase octant
-
- if (p_octant == root) { // won't have a parent, just erase
-
- root = nullptr;
- } else {
- ERR_FAIL_INDEX_V(p_octant->parent_index, 8, octant_removed);
-
- parent->children[p_octant->parent_index] = nullptr;
- parent->children_count--;
- }
-
- memdelete_allocator<Octant, AL>(p_octant);
- octant_count--;
- removed = true;
- octant_removed = true;
- }
-
- if (!removed && !unpaired) {
- return octant_removed; // no reason to keep going up anymore! was already visited and was not removed
- }
-
- p_octant = parent;
- }
-
- return octant_removed;
-}
-
-template <class T, bool use_pairs, class AL>
-void Octree<T, use_pairs, AL>::_unpair_element(Element *p_element, Octant *p_octant) {
- // always test pairable
- typename List<Element *, AL>::Element *E = p_octant->pairable_elements.front();
- while (E) {
- if (E->get()->last_pass != pass) { // only remove ONE reference
- _pair_unreference(p_element, E->get());
- E->get()->last_pass = pass;
- }
- E = E->next();
- }
-
- if (p_element->pairable) {
- // and always test non-pairable if element is pairable
- E = p_octant->elements.front();
- while (E) {
- if (E->get()->last_pass != pass) { // only remove ONE reference
- _pair_unreference(p_element, E->get());
- E->get()->last_pass = pass;
- }
- E = E->next();
- }
- }
-
- p_octant->last_pass = pass;
-
- if (p_octant->children_count == 0) {
- return; // small optimization for leafs
- }
-
- for (int i = 0; i < 8; i++) {
- if (p_octant->children[i]) {
- _unpair_element(p_element, p_octant->children[i]);
- }
- }
-}
-
-template <class T, bool use_pairs, class AL>
-void Octree<T, use_pairs, AL>::_pair_element(Element *p_element, Octant *p_octant) {
- // always test pairable
-
- typename List<Element *, AL>::Element *E = p_octant->pairable_elements.front();
-
- while (E) {
- if (E->get()->last_pass != pass) { // only get ONE reference
- _pair_reference(p_element, E->get());
- E->get()->last_pass = pass;
- }
- E = E->next();
- }
-
- if (p_element->pairable) {
- // and always test non-pairable if element is pairable
- E = p_octant->elements.front();
- while (E) {
- if (E->get()->last_pass != pass) { // only get ONE reference
- _pair_reference(p_element, E->get());
- E->get()->last_pass = pass;
- }
- E = E->next();
- }
- }
- p_octant->last_pass = pass;
-
- if (p_octant->children_count == 0) {
- return; // small optimization for leafs
- }
-
- for (int i = 0; i < 8; i++) {
- if (p_octant->children[i]) {
- _pair_element(p_element, p_octant->children[i]);
- }
- }
-}
-
-template <class T, bool use_pairs, class AL>
-void Octree<T, use_pairs, AL>::_remove_element(Element *p_element) {
- pass++; // will do a new pass for this
-
- typename List<typename Element::OctantOwner, AL>::Element *I = p_element->octant_owners.front();
-
- /* FIRST remove going up normally */
- for (; I; I = I->next()) {
- Octant *o = I->get().octant;
-
- if (!use_pairs) { // small speedup
- o->elements.erase(I->get().E);
- }
-
- _remove_element_from_octant(p_element, o);
- }
-
- /* THEN remove going down */
-
- I = p_element->octant_owners.front();
-
- if (use_pairs) {
- for (; I; I = I->next()) {
- Octant *o = I->get().octant;
-
- // erase children pairs, they are erased ONCE even if repeated
- pass++;
- for (int i = 0; i < 8; i++) {
- if (o->children[i]) {
- _unpair_element(p_element, o->children[i]);
- }
- }
-
- if (p_element->pairable) {
- o->pairable_elements.erase(I->get().E);
- } else {
- o->elements.erase(I->get().E);
- }
- }
- }
-
- p_element->octant_owners.clear();
-
- if (use_pairs) {
- int remaining = p_element->pair_list.size();
- //p_element->pair_list.clear();
- ERR_FAIL_COND(remaining);
- }
-}
-
-template <class T, bool use_pairs, class AL>
-OctreeElementID Octree<T, use_pairs, AL>::create(T *p_userdata, const AABB &p_aabb, int p_subindex, bool p_pairable, uint32_t p_pairable_type, uint32_t p_pairable_mask) {
-// check for AABB validity
-#ifdef DEBUG_ENABLED
- ERR_FAIL_COND_V(p_aabb.position.x > 1e15 || p_aabb.position.x < -1e15, 0);
- ERR_FAIL_COND_V(p_aabb.position.y > 1e15 || p_aabb.position.y < -1e15, 0);
- ERR_FAIL_COND_V(p_aabb.position.z > 1e15 || p_aabb.position.z < -1e15, 0);
- ERR_FAIL_COND_V(p_aabb.size.x > 1e15 || p_aabb.size.x < 0.0, 0);
- ERR_FAIL_COND_V(p_aabb.size.y > 1e15 || p_aabb.size.y < 0.0, 0);
- ERR_FAIL_COND_V(p_aabb.size.z > 1e15 || p_aabb.size.z < 0.0, 0);
- ERR_FAIL_COND_V(Math::is_nan(p_aabb.size.x), 0);
- ERR_FAIL_COND_V(Math::is_nan(p_aabb.size.y), 0);
- ERR_FAIL_COND_V(Math::is_nan(p_aabb.size.z), 0);
-
-#endif
- typename ElementMap::Element *E = element_map.insert(last_element_id++,
- Element());
- Element &e = E->get();
-
- e.aabb = p_aabb;
- e.userdata = p_userdata;
- e.subindex = p_subindex;
- e.last_pass = 0;
- e.octree = this;
- e.pairable = p_pairable;
- e.pairable_type = p_pairable_type;
- e.pairable_mask = p_pairable_mask;
- e._id = last_element_id - 1;
-
- if (!e.aabb.has_no_surface()) {
- _ensure_valid_root(p_aabb);
- _insert_element(&e, root);
- if (use_pairs) {
- _element_check_pairs(&e);
- }
- }
-
- return last_element_id - 1;
-}
-
-template <class T, bool use_pairs, class AL>
-void Octree<T, use_pairs, AL>::move(OctreeElementID p_id, const AABB &p_aabb) {
-#ifdef DEBUG_ENABLED
- // check for AABB validity
- ERR_FAIL_COND(p_aabb.position.x > 1e15 || p_aabb.position.x < -1e15);
- ERR_FAIL_COND(p_aabb.position.y > 1e15 || p_aabb.position.y < -1e15);
- ERR_FAIL_COND(p_aabb.position.z > 1e15 || p_aabb.position.z < -1e15);
- ERR_FAIL_COND(p_aabb.size.x > 1e15 || p_aabb.size.x < 0.0);
- ERR_FAIL_COND(p_aabb.size.y > 1e15 || p_aabb.size.y < 0.0);
- ERR_FAIL_COND(p_aabb.size.z > 1e15 || p_aabb.size.z < 0.0);
- ERR_FAIL_COND(Math::is_nan(p_aabb.size.x));
- ERR_FAIL_COND(Math::is_nan(p_aabb.size.y));
- ERR_FAIL_COND(Math::is_nan(p_aabb.size.z));
-#endif
- typename ElementMap::Element *E = element_map.find(p_id);
- ERR_FAIL_COND(!E);
- Element &e = E->get();
-
- bool old_has_surf = !e.aabb.has_no_surface();
- bool new_has_surf = !p_aabb.has_no_surface();
-
- if (old_has_surf != new_has_surf) {
- if (old_has_surf) {
- _remove_element(&e); // removing
- e.common_parent = nullptr;
- e.aabb = AABB();
- _optimize();
- } else {
- _ensure_valid_root(p_aabb); // inserting
- e.common_parent = nullptr;
- e.aabb = p_aabb;
- _insert_element(&e, root);
- if (use_pairs) {
- _element_check_pairs(&e);
- }
- }
-
- return;
- }
-
- if (!old_has_surf) { // doing nothing
- return;
- }
-
- // it still is enclosed in the same AABB it was assigned to
- if (e.container_aabb.encloses(p_aabb)) {
- e.aabb = p_aabb;
- if (use_pairs) {
- _element_check_pairs(&e); // must check pairs anyway
- }
-
- return;
- }
-
- AABB combined = e.aabb;
- combined.merge_with(p_aabb);
- _ensure_valid_root(combined);
-
- ERR_FAIL_COND(e.octant_owners.front() == nullptr);
-
- /* FIND COMMON PARENT */
-
- List<typename Element::OctantOwner, AL> owners = e.octant_owners; // save the octant owners
- Octant *common_parent = e.common_parent;
- ERR_FAIL_COND(!common_parent);
-
- //src is now the place towards where insertion is going to happen
- pass++;
-
- while (common_parent && !common_parent->aabb.encloses(p_aabb)) {
- common_parent = common_parent->parent;
- }
-
- ERR_FAIL_COND(!common_parent);
-
- //prepare for reinsert
- e.octant_owners.clear();
- e.common_parent = nullptr;
- e.aabb = p_aabb;
-
- _insert_element(&e, common_parent); // reinsert from this point
-
- pass++;
-
- for (typename List<typename Element::OctantOwner, AL>::Element *F = owners.front(); F;) {
- Octant *o = F->get().octant;
- typename List<typename Element::OctantOwner, AL>::Element *N = F->next();
-
- if (use_pairs && e.pairable) {
- o->pairable_elements.erase(F->get().E);
- } else {
- o->elements.erase(F->get().E);
- }
-
- if (_remove_element_from_octant(&e, o, common_parent->parent)) {
- owners.erase(F);
- }
-
- F = N;
- }
-
- if (use_pairs) {
- //unpair child elements in anything that survived
- for (typename List<typename Element::OctantOwner, AL>::Element *F = owners.front(); F; F = F->next()) {
- Octant *o = F->get().octant;
-
- // erase children pairs, unref ONCE
- pass++;
- for (int i = 0; i < 8; i++) {
- if (o->children[i]) {
- _unpair_element(&e, o->children[i]);
- }
- }
- }
-
- _element_check_pairs(&e);
- }
-
- _optimize();
-}
-
-template <class T, bool use_pairs, class AL>
-void Octree<T, use_pairs, AL>::set_pairable(OctreeElementID p_id, bool p_pairable, uint32_t p_pairable_type, uint32_t p_pairable_mask) {
- typename ElementMap::Element *E = element_map.find(p_id);
- ERR_FAIL_COND(!E);
-
- Element &e = E->get();
-
- if (p_pairable == e.pairable && e.pairable_type == p_pairable_type && e.pairable_mask == p_pairable_mask) {
- return; // no changes, return
- }
-
- if (!e.aabb.has_no_surface()) {
- _remove_element(&e);
- }
-
- e.pairable = p_pairable;
- e.pairable_type = p_pairable_type;
- e.pairable_mask = p_pairable_mask;
- e.common_parent = nullptr;
-
- if (!e.aabb.has_no_surface()) {
- _ensure_valid_root(e.aabb);
- _insert_element(&e, root);
- if (use_pairs) {
- _element_check_pairs(&e);
- }
- }
-}
-
-template <class T, bool use_pairs, class AL>
-void Octree<T, use_pairs, AL>::erase(OctreeElementID p_id) {
- typename ElementMap::Element *E = element_map.find(p_id);
- ERR_FAIL_COND(!E);
-
- Element &e = E->get();
-
- if (!e.aabb.has_no_surface()) {
- _remove_element(&e);
- }
-
- element_map.erase(p_id);
- _optimize();
-}
-
-template <class T, bool use_pairs, class AL>
-void Octree<T, use_pairs, AL>::_cull_convex(Octant *p_octant, _CullConvexData *p_cull) {
- if (*p_cull->result_idx == p_cull->result_max) {
- return; //pointless
- }
-
- if (!p_octant->elements.is_empty()) {
- typename List<Element *, AL>::Element *I;
- I = p_octant->elements.front();
-
- for (; I; I = I->next()) {
- Element *e = I->get();
-
- if (e->last_pass == pass || (use_pairs && !(e->pairable_type & p_cull->mask))) {
- continue;
- }
- e->last_pass = pass;
-
- if (e->aabb.intersects_convex_shape(p_cull->planes, p_cull->plane_count, p_cull->points, p_cull->point_count)) {
- if (*p_cull->result_idx < p_cull->result_max) {
- p_cull->result_array[*p_cull->result_idx] = e->userdata;
- (*p_cull->result_idx)++;
- } else {
- return; // pointless to continue
- }
- }
- }
- }
-
- if (use_pairs && !p_octant->pairable_elements.is_empty()) {
- typename List<Element *, AL>::Element *I;
- I = p_octant->pairable_elements.front();
-
- for (; I; I = I->next()) {
- Element *e = I->get();
-
- if (e->last_pass == pass || (use_pairs && !(e->pairable_type & p_cull->mask))) {
- continue;
- }
- e->last_pass = pass;
-
- if (e->aabb.intersects_convex_shape(p_cull->planes, p_cull->plane_count, p_cull->points, p_cull->point_count)) {
- if (*p_cull->result_idx < p_cull->result_max) {
- p_cull->result_array[*p_cull->result_idx] = e->userdata;
- (*p_cull->result_idx)++;
- } else {
- return; // pointless to continue
- }
- }
- }
- }
-
- for (int i = 0; i < 8; i++) {
- if (p_octant->children[i] && p_octant->children[i]->aabb.intersects_convex_shape(p_cull->planes, p_cull->plane_count, p_cull->points, p_cull->point_count)) {
- _cull_convex(p_octant->children[i], p_cull);
- }
- }
-}
-
-template <class T, bool use_pairs, class AL>
-void Octree<T, use_pairs, AL>::_cull_aabb(Octant *p_octant, const AABB &p_aabb, T **p_result_array, int *p_result_idx, int p_result_max, int *p_subindex_array, uint32_t p_mask) {
- if (*p_result_idx == p_result_max) {
- return; //pointless
- }
-
- if (!p_octant->elements.is_empty()) {
- typename List<Element *, AL>::Element *I;
- I = p_octant->elements.front();
- for (; I; I = I->next()) {
- Element *e = I->get();
-
- if (e->last_pass == pass || (use_pairs && !(e->pairable_type & p_mask))) {
- continue;
- }
- e->last_pass = pass;
-
- if (p_aabb.intersects_inclusive(e->aabb)) {
- if (*p_result_idx < p_result_max) {
- p_result_array[*p_result_idx] = e->userdata;
- if (p_subindex_array) {
- p_subindex_array[*p_result_idx] = e->subindex;
- }
-
- (*p_result_idx)++;
- } else {
- return; // pointless to continue
- }
- }
- }
- }
-
- if (use_pairs && !p_octant->pairable_elements.is_empty()) {
- typename List<Element *, AL>::Element *I;
- I = p_octant->pairable_elements.front();
- for (; I; I = I->next()) {
- Element *e = I->get();
-
- if (e->last_pass == pass || (use_pairs && !(e->pairable_type & p_mask))) {
- continue;
- }
- e->last_pass = pass;
-
- if (p_aabb.intersects_inclusive(e->aabb)) {
- if (*p_result_idx < p_result_max) {
- p_result_array[*p_result_idx] = e->userdata;
- if (p_subindex_array) {
- p_subindex_array[*p_result_idx] = e->subindex;
- }
- (*p_result_idx)++;
- } else {
- return; // pointless to continue
- }
- }
- }
- }
-
- for (int i = 0; i < 8; i++) {
- if (p_octant->children[i] && p_octant->children[i]->aabb.intersects_inclusive(p_aabb)) {
- _cull_aabb(p_octant->children[i], p_aabb, p_result_array, p_result_idx, p_result_max, p_subindex_array, p_mask);
- }
- }
-}
-
-template <class T, bool use_pairs, class AL>
-void Octree<T, use_pairs, AL>::_cull_segment(Octant *p_octant, const Vector3 &p_from, const Vector3 &p_to, T **p_result_array, int *p_result_idx, int p_result_max, int *p_subindex_array, uint32_t p_mask) {
- if (*p_result_idx == p_result_max) {
- return; //pointless
- }
-
- if (!p_octant->elements.is_empty()) {
- typename List<Element *, AL>::Element *I;
- I = p_octant->elements.front();
- for (; I; I = I->next()) {
- Element *e = I->get();
-
- if (e->last_pass == pass || (use_pairs && !(e->pairable_type & p_mask))) {
- continue;
- }
- e->last_pass = pass;
-
- if (e->aabb.intersects_segment(p_from, p_to)) {
- if (*p_result_idx < p_result_max) {
- p_result_array[*p_result_idx] = e->userdata;
- if (p_subindex_array) {
- p_subindex_array[*p_result_idx] = e->subindex;
- }
- (*p_result_idx)++;
-
- } else {
- return; // pointless to continue
- }
- }
- }
- }
-
- if (use_pairs && !p_octant->pairable_elements.is_empty()) {
- typename List<Element *, AL>::Element *I;
- I = p_octant->pairable_elements.front();
- for (; I; I = I->next()) {
- Element *e = I->get();
-
- if (e->last_pass == pass || (use_pairs && !(e->pairable_type & p_mask))) {
- continue;
- }
-
- e->last_pass = pass;
-
- if (e->aabb.intersects_segment(p_from, p_to)) {
- if (*p_result_idx < p_result_max) {
- p_result_array[*p_result_idx] = e->userdata;
- if (p_subindex_array) {
- p_subindex_array[*p_result_idx] = e->subindex;
- }
-
- (*p_result_idx)++;
-
- } else {
- return; // pointless to continue
- }
- }
- }
- }
-
- for (int i = 0; i < 8; i++) {
- if (p_octant->children[i] && p_octant->children[i]->aabb.intersects_segment(p_from, p_to)) {
- _cull_segment(p_octant->children[i], p_from, p_to, p_result_array, p_result_idx, p_result_max, p_subindex_array, p_mask);
- }
- }
-}
-
-template <class T, bool use_pairs, class AL>
-void Octree<T, use_pairs, AL>::_cull_point(Octant *p_octant, const Vector3 &p_point, T **p_result_array, int *p_result_idx, int p_result_max, int *p_subindex_array, uint32_t p_mask) {
- if (*p_result_idx == p_result_max) {
- return; //pointless
- }
-
- if (!p_octant->elements.is_empty()) {
- typename List<Element *, AL>::Element *I;
- I = p_octant->elements.front();
- for (; I; I = I->next()) {
- Element *e = I->get();
-
- if (e->last_pass == pass || (use_pairs && !(e->pairable_type & p_mask))) {
- continue;
- }
- e->last_pass = pass;
-
- if (e->aabb.has_point(p_point)) {
- if (*p_result_idx < p_result_max) {
- p_result_array[*p_result_idx] = e->userdata;
- if (p_subindex_array) {
- p_subindex_array[*p_result_idx] = e->subindex;
- }
- (*p_result_idx)++;
-
- } else {
- return; // pointless to continue
- }
- }
- }
- }
-
- if (use_pairs && !p_octant->pairable_elements.is_empty()) {
- typename List<Element *, AL>::Element *I;
- I = p_octant->pairable_elements.front();
- for (; I; I = I->next()) {
- Element *e = I->get();
-
- if (e->last_pass == pass || (use_pairs && !(e->pairable_type & p_mask))) {
- continue;
- }
-
- e->last_pass = pass;
-
- if (e->aabb.has_point(p_point)) {
- if (*p_result_idx < p_result_max) {
- p_result_array[*p_result_idx] = e->userdata;
- if (p_subindex_array) {
- p_subindex_array[*p_result_idx] = e->subindex;
- }
-
- (*p_result_idx)++;
-
- } else {
- return; // pointless to continue
- }
- }
- }
- }
-
- for (int i = 0; i < 8; i++) {
- //could be optimized..
- if (p_octant->children[i] && p_octant->children[i]->aabb.has_point(p_point)) {
- _cull_point(p_octant->children[i], p_point, p_result_array, p_result_idx, p_result_max, p_subindex_array, p_mask);
- }
- }
-}
-
-template <class T, bool use_pairs, class AL>
-int Octree<T, use_pairs, AL>::cull_convex(const Vector<Plane> &p_convex, T **p_result_array, int p_result_max, uint32_t p_mask) {
- if (!root || p_convex.size() == 0) {
- return 0;
- }
-
- Vector<Vector3> convex_points = Geometry3D::compute_convex_mesh_points(&p_convex[0], p_convex.size());
- if (convex_points.size() == 0) {
- return 0;
- }
-
- int result_count = 0;
- pass++;
- _CullConvexData cdata;
- cdata.planes = &p_convex[0];
- cdata.plane_count = p_convex.size();
- cdata.points = &convex_points[0];
- cdata.point_count = convex_points.size();
- cdata.result_array = p_result_array;
- cdata.result_max = p_result_max;
- cdata.result_idx = &result_count;
- cdata.mask = p_mask;
-
- _cull_convex(root, &cdata);
-
- return result_count;
-}
-
-template <class T, bool use_pairs, class AL>
-int Octree<T, use_pairs, AL>::cull_aabb(const AABB &p_aabb, T **p_result_array, int p_result_max, int *p_subindex_array, uint32_t p_mask) {
- if (!root) {
- return 0;
- }
-
- int result_count = 0;
- pass++;
- _cull_aabb(root, p_aabb, p_result_array, &result_count, p_result_max, p_subindex_array, p_mask);
-
- return result_count;
-}
-
-template <class T, bool use_pairs, class AL>
-int Octree<T, use_pairs, AL>::cull_segment(const Vector3 &p_from, const Vector3 &p_to, T **p_result_array, int p_result_max, int *p_subindex_array, uint32_t p_mask) {
- if (!root) {
- return 0;
- }
-
- int result_count = 0;
- pass++;
- _cull_segment(root, p_from, p_to, p_result_array, &result_count, p_result_max, p_subindex_array, p_mask);
-
- return result_count;
-}
-
-template <class T, bool use_pairs, class AL>
-int Octree<T, use_pairs, AL>::cull_point(const Vector3 &p_point, T **p_result_array, int p_result_max, int *p_subindex_array, uint32_t p_mask) {
- if (!root) {
- return 0;
- }
-
- int result_count = 0;
- pass++;
- _cull_point(root, p_point, p_result_array, &result_count, p_result_max, p_subindex_array, p_mask);
-
- return result_count;
-}
-
-template <class T, bool use_pairs, class AL>
-void Octree<T, use_pairs, AL>::set_pair_callback(PairCallback p_callback, void *p_userdata) {
- pair_callback = p_callback;
- pair_callback_userdata = p_userdata;
-}
-
-template <class T, bool use_pairs, class AL>
-void Octree<T, use_pairs, AL>::set_unpair_callback(UnpairCallback p_callback, void *p_userdata) {
- unpair_callback = p_callback;
- unpair_callback_userdata = p_userdata;
-}
-
-template <class T, bool use_pairs, class AL>
-Octree<T, use_pairs, AL>::Octree(real_t p_unit_size) {
- unit_size = p_unit_size;
-}
-
-#endif // OCTREE_H
diff --git a/core/math/vector2.cpp b/core/math/vector2.cpp
index a27227905c..d9b5d55454 100644
--- a/core/math/vector2.cpp
+++ b/core/math/vector2.cpp
@@ -152,13 +152,6 @@ Vector2 Vector2::limit_length(const real_t p_len) const {
return v;
}
-Vector2 Vector2::cubic_interpolate(const Vector2 &p_b, const Vector2 &p_pre_a, const Vector2 &p_post_b, const 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::move_toward(const Vector2 &p_to, const real_t p_delta) const {
Vector2 v = *this;
Vector2 vd = p_to - v;
diff --git a/core/math/vector2.h b/core/math/vector2.h
index bd67299f33..91d3d3a56b 100644
--- a/core/math/vector2.h
+++ b/core/math/vector2.h
@@ -113,7 +113,9 @@ struct _NO_DISCARD_ Vector2 {
_FORCE_INLINE_ Vector2 lerp(const Vector2 &p_to, const real_t p_weight) const;
_FORCE_INLINE_ Vector2 slerp(const Vector2 &p_to, const real_t p_weight) const;
- Vector2 cubic_interpolate(const Vector2 &p_b, const Vector2 &p_pre_a, const Vector2 &p_post_b, const real_t p_weight) const;
+ _FORCE_INLINE_ Vector2 cubic_interpolate(const Vector2 &p_b, const Vector2 &p_pre_a, const Vector2 &p_post_b, const real_t p_weight) const;
+ _FORCE_INLINE_ Vector2 bezier_interpolate(const Vector2 &p_control_1, const Vector2 &p_control_2, const Vector2 &p_end, const real_t p_t) const;
+
Vector2 move_toward(const Vector2 &p_to, const real_t p_delta) const;
Vector2 slide(const Vector2 &p_normal) const;
@@ -261,6 +263,26 @@ Vector2 Vector2::slerp(const Vector2 &p_to, const real_t p_weight) const {
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, const 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::bezier_interpolate(const Vector2 &p_control_1, const Vector2 &p_control_2, const Vector2 &p_end, const real_t p_t) const {
+ Vector2 res = *this;
+
+ /* Formula from Wikipedia article on Bezier curves. */
+ real_t omt = (1.0 - p_t);
+ real_t omt2 = omt * omt;
+ real_t omt3 = omt2 * omt;
+ real_t t2 = p_t * p_t;
+ real_t t3 = t2 * p_t;
+
+ return res * omt3 + p_control_1 * omt2 * p_t * 3.0 + p_control_2 * omt * t2 * 3.0 + p_end * t3;
+}
+
Vector2 Vector2::direction_to(const Vector2 &p_to) const {
Vector2 ret(p_to.x - x, p_to.y - y);
ret.normalize();
diff --git a/core/math/vector3.cpp b/core/math/vector3.cpp
index f94f39b7f2..d71d365053 100644
--- a/core/math/vector3.cpp
+++ b/core/math/vector3.cpp
@@ -85,14 +85,6 @@ Vector3 Vector3::limit_length(const real_t p_len) const {
return v;
}
-Vector3 Vector3::cubic_interpolate(const Vector3 &p_b, const Vector3 &p_pre_a, const Vector3 &p_post_b, const real_t p_weight) const {
- Vector3 res = *this;
- res.x = Math::cubic_interpolate(res.x, p_b.x, p_pre_a.x, p_post_b.x, p_weight);
- res.y = Math::cubic_interpolate(res.y, p_b.y, p_pre_a.y, p_post_b.y, p_weight);
- res.z = Math::cubic_interpolate(res.z, p_b.z, p_pre_a.z, p_post_b.z, p_weight);
- return res;
-}
-
Vector3 Vector3::move_toward(const Vector3 &p_to, const real_t p_delta) const {
Vector3 v = *this;
Vector3 vd = p_to - v;
diff --git a/core/math/vector3.h b/core/math/vector3.h
index 8891532f42..970416234d 100644
--- a/core/math/vector3.h
+++ b/core/math/vector3.h
@@ -104,7 +104,9 @@ struct _NO_DISCARD_ Vector3 {
_FORCE_INLINE_ Vector3 lerp(const Vector3 &p_to, const real_t p_weight) const;
_FORCE_INLINE_ Vector3 slerp(const Vector3 &p_to, const real_t p_weight) const;
- Vector3 cubic_interpolate(const Vector3 &p_b, const Vector3 &p_pre_a, const Vector3 &p_post_b, const real_t p_weight) const;
+ _FORCE_INLINE_ Vector3 cubic_interpolate(const Vector3 &p_b, const Vector3 &p_pre_a, const Vector3 &p_post_b, const real_t p_weight) const;
+ _FORCE_INLINE_ Vector3 bezier_interpolate(const Vector3 &p_control_1, const Vector3 &p_control_2, const Vector3 &p_end, const real_t p_t) const;
+
Vector3 move_toward(const Vector3 &p_to, const real_t p_delta) const;
Vector2 octahedron_encode() const;
@@ -227,6 +229,27 @@ Vector3 Vector3::slerp(const Vector3 &p_to, const real_t p_weight) const {
return rotated(cross(p_to).normalized(), 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, const 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::bezier_interpolate(const Vector3 &p_control_1, const Vector3 &p_control_2, const Vector3 &p_end, const real_t p_t) const {
+ Vector3 res = *this;
+
+ /* Formula from Wikipedia article on Bezier curves. */
+ real_t omt = (1.0 - p_t);
+ real_t omt2 = omt * omt;
+ real_t omt3 = omt2 * omt;
+ real_t t2 = p_t * p_t;
+ real_t t3 = t2 * p_t;
+
+ return res * omt3 + p_control_1 * omt2 * p_t * 3.0 + p_control_2 * omt * t2 * 3.0 + p_end * t3;
+}
+
real_t Vector3::distance_to(const Vector3 &p_to) const {
return (p_to - *this).length();
}
generated by cgit v1.2.3 (git 2.25.1) at 2025-03-22 09:43:22 +0000