diff --git a/core/math/bvh.h b/core/math/bvh.h index 002e11c5a53..70dde53e839 100644 --- a/core/math/bvh.h +++ b/core/math/bvh.h @@ -48,9 +48,9 @@ #include "bvh_tree.h" -#define BVHTREE_CLASS BVH_Tree +#define BVHTREE_CLASS BVH_Tree -template +template class BVH_Manager { public: @@ -88,7 +88,7 @@ public: unpair_callback_userdata = p_userdata; } - BVHHandle create(T *p_userdata, bool p_active, const AABB &p_aabb = AABB(), int p_subindex = 0, bool p_pairable = false, uint32_t p_pairable_type = 0, uint32_t p_pairable_mask = 1) { + BVHHandle create(T *p_userdata, bool p_active, const Bounds &p_aabb = Bounds(), int p_subindex = 0, bool p_pairable = false, uint32_t p_pairable_type = 0, uint32_t p_pairable_mask = 1) { // 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. @@ -108,7 +108,7 @@ public: if (USE_PAIRS) { // for safety initialize the expanded AABB - AABB &expanded_aabb = tree._pairs[h.id()].expanded_aabb; + Bounds &expanded_aabb = tree._pairs[h.id()].expanded_aabb; expanded_aabb = p_aabb; expanded_aabb.grow_by(tree._pairing_expansion); @@ -125,7 +125,7 @@ public: //////////////////////////////////////////////////// // wrapper versions that use uint32_t instead of handle // for backward compatibility. Less type safe - void move(uint32_t p_handle, const AABB &p_aabb) { + void move(uint32_t p_handle, const Bounds &p_aabb) { BVHHandle h; h.set(p_handle); move(h, p_aabb); @@ -143,7 +143,7 @@ public: force_collision_check(h); } - bool activate(uint32_t p_handle, const AABB &p_aabb, bool p_delay_collision_check = false) { + 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); @@ -180,7 +180,7 @@ public: //////////////////////////////////////////////////// - void move(BVHHandle p_handle, const AABB &p_aabb) { + void move(BVHHandle p_handle, const Bounds &p_aabb) { if (tree.item_move(p_handle, p_aabb)) { if (USE_PAIRS) { @@ -207,7 +207,7 @@ public: void force_collision_check(BVHHandle p_handle) { if (USE_PAIRS) { // the aabb should already be up to date in the BVH - AABB aabb; + Bounds aabb; item_get_AABB(p_handle, aabb); // add it as changed even if aabb not different @@ -221,7 +221,7 @@ public: // 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 AABB &p_aabb, bool p_delay_collision_check = false) { + bool activate(BVHHandle p_handle, const Bounds &p_aabb, bool p_delay_collision_check = false) { // sending the aabb here prevents the need for the BVH to maintain // a redundant copy of the aabb. // returns success @@ -294,7 +294,7 @@ public: // 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. - AABB aabb; + Bounds aabb; item_get_AABB(p_handle, aabb); // passing false disables the optimization which prevents collision checks if @@ -311,7 +311,7 @@ public: } // cull tests - 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_aabb(const Bounds &p_aabb, T **p_result_array, int p_result_max, int *p_subindex_array = nullptr, uint32_t p_mask = 0xFFFFFFFF) { typename BVHTREE_CLASS::CullParams params; params.result_count_overall = 0; @@ -328,7 +328,7 @@ public: return params.result_count_overall; } - 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_segment(const Point &p_from, const Point &p_to, T **p_result_array, int p_result_max, int *p_subindex_array = nullptr, uint32_t p_mask = 0xFFFFFFFF) { typename BVHTREE_CLASS::CullParams params; params.result_count_overall = 0; @@ -346,7 +346,7 @@ public: return params.result_count_overall; } - 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) { + int cull_point(const Point &p_point, T **p_result_array, int p_result_max, int *p_subindex_array = nullptr, uint32_t p_mask = 0xFFFFFFFF) { typename BVHTREE_CLASS::CullParams params; params.result_count_overall = 0; @@ -396,7 +396,7 @@ private: return; } - AABB bb; + Bounds bb; typename BVHTREE_CLASS::CullParams params; @@ -411,8 +411,8 @@ private: const BVHHandle &h = changed_items[n]; // use the expanded aabb for pairing - const AABB &expanded_aabb = tree._pairs[h.id()].expanded_aabb; - BVH_ABB abb; + const Bounds &expanded_aabb = tree._pairs[h.id()].expanded_aabb; + BVHABB_CLASS abb; abb.from(expanded_aabb); // find all the existing paired aabbs that are no longer @@ -457,8 +457,8 @@ private: } public: - void item_get_AABB(BVHHandle p_handle, AABB &r_aabb) { - BVH_ABB abb; + void item_get_AABB(BVHHandle p_handle, Bounds &r_aabb) { + BVHABB_CLASS abb; tree.item_get_ABB(p_handle, abb); abb.to(r_aabb); } @@ -494,8 +494,8 @@ private: } // returns true if unpair - bool _find_leavers_process_pair(typename BVHTREE_CLASS::ItemPairs &p_pairs_from, const BVH_ABB &p_abb_from, BVHHandle p_from, BVHHandle p_to, bool p_full_check) { - BVH_ABB abb_to; + 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? @@ -526,10 +526,10 @@ private: // find all the existing paired aabbs that are no longer // paired, and send callbacks - void _find_leavers(BVHHandle p_handle, const BVH_ABB &expanded_abb_from, bool p_full_check) { + 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()]; - BVH_ABB abb_from = expanded_abb_from; + 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++) { @@ -608,7 +608,7 @@ private: _tick++; } - void _add_changed_item(BVHHandle p_handle, const AABB &aabb, bool p_check_aabb = true) { + 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 @@ -616,7 +616,7 @@ private: // 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 - AABB &expanded_aabb = tree._pairs[p_handle.id()].expanded_aabb; + 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 diff --git a/core/math/bvh_abb.h b/core/math/bvh_abb.h index fba9863839e..4c825da23d9 100644 --- a/core/math/bvh_abb.h +++ b/core/math/bvh_abb.h @@ -32,6 +32,7 @@ #define BVH_ABB_H // special optimized version of axis aligned bounding box +template struct BVH_ABB { struct ConvexHull { // convex hulls (optional) @@ -42,8 +43,8 @@ struct BVH_ABB { }; struct Segment { - Vector3 from; - Vector3 to; + Point from; + Point to; }; enum IntersectResult { @@ -53,49 +54,50 @@ struct BVH_ABB { }; // we store mins with a negative value in order to test them with SIMD - Vector3 min; - Vector3 neg_max; + 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 Vector3 &_min, const Vector3 &_max) { + void set(const Point &_min, const Point &_max) { min = _min; neg_max = -_max; } // to and from standard AABB - void from(const AABB &p_aabb) { + void from(const Bounds &p_aabb) { min = p_aabb.position; neg_max = -(p_aabb.position + p_aabb.size); } - void to(AABB &r_aabb) const { + void to(Bounds &r_aabb) const { r_aabb.position = min; r_aabb.size = calculate_size(); } void merge(const BVH_ABB &p_o) { - neg_max.x = MIN(neg_max.x, p_o.neg_max.x); - neg_max.y = MIN(neg_max.y, p_o.neg_max.y); - neg_max.z = MIN(neg_max.z, p_o.neg_max.z); - - min.x = MIN(min.x, p_o.min.x); - min.y = MIN(min.y, p_o.min.y); - min.z = MIN(min.z, p_o.min.z); + 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]); + } } - Vector3 calculate_size() const { + Point calculate_size() const { return -neg_max - min; } - Vector3 calculate_centre() const { - return Vector3((calculate_size() * 0.5) + min); + Point calculate_centre() const { + return Point((calculate_size() * 0.5) + min); } real_t get_proximity_to(const BVH_ABB &p_b) const { - const Vector3 d = (min - neg_max) - (p_b.min - p_b.neg_max); - return (Math::abs(d.x) + Math::abs(d.y) + Math::abs(d.z)); + 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 { @@ -158,7 +160,7 @@ struct BVH_ABB { } bool intersects_convex_partial(const ConvexHull &p_hull) const { - AABB bb; + Bounds bb; to(bb); return bb.intersects_convex_shape(p_hull.planes, p_hull.num_planes, p_hull.points, p_hull.num_points); } @@ -178,7 +180,7 @@ struct BVH_ABB { bool is_within_convex(const ConvexHull &p_hull) const { // use half extents routine - AABB bb; + Bounds bb; to(bb); return bb.inside_convex_shape(p_hull.planes, p_hull.num_planes); } @@ -192,59 +194,66 @@ struct BVH_ABB { } bool intersects_segment(const Segment &p_s) const { - AABB bb; + Bounds bb; to(bb); return bb.intersects_segment(p_s.from, p_s.to); } - bool intersects_point(const Vector3 &p_pt) const { - if (_vector3_any_lessthan(-p_pt, neg_max)) return false; - if (_vector3_any_lessthan(p_pt, min)) return false; + 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; } bool intersects(const BVH_ABB &p_o) const { - if (_vector3_any_morethan(p_o.min, -neg_max)) return false; - if (_vector3_any_morethan(min, -p_o.neg_max)) return false; + if (_any_morethan(p_o.min, -neg_max)) return false; + if (_any_morethan(min, -p_o.neg_max)) return false; return true; } bool is_other_within(const BVH_ABB &p_o) const { - if (_vector3_any_lessthan(p_o.neg_max, neg_max)) return false; - if (_vector3_any_lessthan(p_o.min, min)) return false; + 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 Vector3 &p_change) { + void grow(const Point &p_change) { neg_max -= p_change; min -= p_change; } void expand(real_t p_change) { - grow(Vector3(p_change, p_change, p_change)); + Point change; + change.set_all(p_change); + grow(change); } - float get_area() const // actually surface area metric - { - Vector3 d = calculate_size(); + // Actually surface area metric. + float 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() { - neg_max = Vector3(FLT_MAX, FLT_MAX, FLT_MAX); + neg_max.set_all(FLT_MAX); min = neg_max; } - bool _vector3_any_morethan(const Vector3 &p_a, const Vector3 &p_b) const { - if (p_a.x > p_b.x) return true; - if (p_a.y > p_b.y) return true; - if (p_a.z > p_b.z) return true; + 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 _vector3_any_lessthan(const Vector3 &p_a, const Vector3 &p_b) const { - if (p_a.x < p_b.x) return true; - if (p_a.y < p_b.y) return true; - if (p_a.z < p_b.z) return true; + 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; } }; diff --git a/core/math/bvh_cull.inc b/core/math/bvh_cull.inc index 3083df550e4..94e57b6d97b 100644 --- a/core/math/bvh_cull.inc +++ b/core/math/bvh_cull.inc @@ -15,9 +15,9 @@ struct CullParams { // optional components for different tests Vector3 point; - BVH_ABB abb; - typename BVH_ABB::ConvexHull hull; - typename BVH_ABB::Segment segment; + BVHABB_CLASS abb; + typename BVHABB_CLASS::ConvexHull hull; + typename BVHABB_CLASS::Segment segment; // when collision testing, non pairable moving items // only need to be tested against the pairable tree. @@ -200,7 +200,7 @@ bool _cull_segment_iterative(uint32_t p_node_id, CullParams &r_params) { // test children individually for (int n = 0; n < leaf.num_items; n++) { - const BVH_ABB &aabb = leaf.get_aabb(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); @@ -214,7 +214,7 @@ bool _cull_segment_iterative(uint32_t p_node_id, CullParams &r_params) { for (int n = 0; n < tnode.num_children; n++) { uint32_t child_id = tnode.children[n]; - const BVH_ABB &child_abb = _nodes[child_id].aabb; + const BVHABB_CLASS &child_abb = _nodes[child_id].aabb; if (child_abb.intersects_segment(r_params.segment)) { @@ -339,7 +339,7 @@ bool _cull_aabb_iterative(uint32_t p_node_id, CullParams &r_params, bool p_fully } } else { for (int n = 0; n < leaf.num_items; n++) { - const BVH_ABB &aabb = leaf.get_aabb(n); + const BVHABB_CLASS &aabb = leaf.get_aabb(n); if (aabb.intersects(r_params.abb)) { uint32_t child_id = leaf.get_item_ref_id(n); @@ -355,7 +355,7 @@ bool _cull_aabb_iterative(uint32_t p_node_id, CullParams &r_params, bool p_fully for (int n = 0; n < tnode.num_children; n++) { uint32_t child_id = tnode.children[n]; - const BVH_ABB &child_abb = _nodes[child_id].aabb; + const BVHABB_CLASS &child_abb = _nodes[child_id].aabb; if (child_abb.intersects(r_params.abb)) { // is the node totally within the aabb? @@ -421,15 +421,15 @@ bool _cull_convex_iterative(uint32_t p_node_id, CullParams &r_params, bool p_ful if (!ccp.fully_within) { - typename BVH_ABB::IntersectResult res = tnode.aabb.intersects_convex(r_params.hull); + 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 BVH_ABB::IR_PARTIAL: { + case BVHABB_CLASS::IR_PARTIAL: { } break; - case BVH_ABB::IR_FULL: { + case BVHABB_CLASS::IR_FULL: { ccp.fully_within = true; } break; } @@ -478,7 +478,7 @@ bool _cull_convex_iterative(uint32_t p_node_id, CullParams &r_params, bool p_ful // test children individually for (int n = 0; n < leaf.num_items; n++) { //const Item &item = leaf.get_item(n); - const BVH_ABB &aabb = leaf.get_aabb(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); @@ -496,7 +496,7 @@ bool _cull_convex_iterative(uint32_t p_node_id, CullParams &r_params, bool p_ful uint32_t test_count = 0; for (int n = 0; n < leaf.num_items; n++) { - const BVH_ABB &aabb = leaf.get_aabb(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); @@ -511,7 +511,7 @@ bool _cull_convex_iterative(uint32_t p_node_id, CullParams &r_params, bool p_ful // not BVH_CONVEX_CULL_OPTIMIZED // test children individually for (int n = 0; n < leaf.num_items; n++) { - const BVH_ABB &aabb = leaf.get_aabb(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); diff --git a/core/math/bvh_debug.inc b/core/math/bvh_debug.inc index 6ff5be54ff8..a97304334cd 100644 --- a/core/math/bvh_debug.inc +++ b/core/math/bvh_debug.inc @@ -5,7 +5,7 @@ void _debug_recursive_print_tree(int p_tree_id) const { _debug_recursive_print_tree_node(_root_node_id[p_tree_id]); } -String _debug_aabb_to_string(const BVH_ABB &aabb) const { +String _debug_aabb_to_string(const BVHABB_CLASS &aabb) const { String sz = "("; sz += itos(aabb.min.x); sz += " ~ "; diff --git a/core/math/bvh_integrity.inc b/core/math/bvh_integrity.inc index 724fc8e2211..02e9d30097d 100644 --- a/core/math/bvh_integrity.inc +++ b/core/math/bvh_integrity.inc @@ -12,10 +12,10 @@ void _integrity_check_all() { void _integrity_check_up(uint32_t p_node_id) { TNode &node = _nodes[p_node_id]; - BVH_ABB abb = node.aabb; + BVHABB_CLASS abb = node.aabb; node_update_aabb(node); - BVH_ABB abb2 = node.aabb; + BVHABB_CLASS abb2 = node.aabb; abb2.expand(-_node_expansion); CRASH_COND(!abb.is_other_within(abb2)); diff --git a/core/math/bvh_logic.inc b/core/math/bvh_logic.inc index 24a4d2383ac..6b96915f151 100644 --- a/core/math/bvh_logic.inc +++ b/core/math/bvh_logic.inc @@ -21,7 +21,7 @@ void _logic_item_remove_and_reinsert(uint32_t p_ref_id) { _current_tree = _handle_get_tree_id(temp_handle); // remove and reinsert - BVH_ABB abb; + BVHABB_CLASS abb; node_remove_item(p_ref_id, &abb); // we must choose where to add to tree @@ -32,8 +32,8 @@ void _logic_item_remove_and_reinsert(uint32_t p_ref_id) { } // from randy gaul balance function -BVH_ABB _logic_abb_merge(const BVH_ABB &a, const BVH_ABB &b) { - BVH_ABB c = a; +BVHABB_CLASS _logic_abb_merge(const BVHABB_CLASS &a, const BVHABB_CLASS &b) { + BVHABB_CLASS c = a; c.merge(b); return c; } @@ -192,7 +192,7 @@ int32_t _logic_balance(int32_t 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 BVH_ABB &p_aabb) { +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); diff --git a/core/math/bvh_pair.inc b/core/math/bvh_pair.inc index ced3022a538..839db59a3aa 100644 --- a/core/math/bvh_pair.inc +++ b/core/math/bvh_pair.inc @@ -14,10 +14,10 @@ struct ItemPairs { void clear() { num_pairs = 0; extended_pairs.reset(); - expanded_aabb = AABB(); + expanded_aabb = Bounds(); } - AABB expanded_aabb; + Bounds expanded_aabb; // maybe we can just use the number in the vector TODO int32_t num_pairs; diff --git a/core/math/bvh_public.inc b/core/math/bvh_public.inc index 1d384e2da57..a35668be1ba 100644 --- a/core/math/bvh_public.inc +++ b/core/math/bvh_public.inc @@ -1,12 +1,12 @@ public: -BVHHandle item_add(T *p_userdata, bool p_active, const AABB &p_aabb, int32_t p_subindex, bool p_pairable, uint32_t p_pairable_type, uint32_t p_pairable_mask, bool p_invisible = false) { +BVHHandle item_add(T *p_userdata, bool p_active, const Bounds &p_aabb, int32_t p_subindex, bool p_pairable, uint32_t p_pairable_type, uint32_t p_pairable_mask, bool p_invisible = false) { #ifdef BVH_VERBOSE_TREE VERBOSE_PRINT("\nitem_add BEFORE"); _debug_recursive_print_tree(0); VERBOSE_PRINT("\n"); #endif - BVH_ABB abb; + BVHABB_CLASS abb; abb.from(p_aabb); // handle to be filled with the new item ref @@ -101,7 +101,7 @@ void _debug_print_refs() { } // returns false if noop -bool item_move(BVHHandle p_handle, const AABB &p_aabb) { +bool item_move(BVHHandle p_handle, const Bounds &p_aabb) { uint32_t ref_id = p_handle.id(); // get the reference @@ -110,7 +110,7 @@ bool item_move(BVHHandle p_handle, const AABB &p_aabb) { return false; } - BVH_ABB abb; + BVHABB_CLASS abb; abb.from(p_aabb); BVH_ASSERT(ref.tnode_id != BVHCommon::INVALID); @@ -124,7 +124,7 @@ bool item_move(BVHHandle p_handle, const AABB &p_aabb) { // for accurate collision detection TLeaf &leaf = _node_get_leaf(tnode); - BVH_ABB &leaf_abb = leaf.get_aabb(ref.item_id); + BVHABB_CLASS &leaf_abb = leaf.get_aabb(ref.item_id); // no change? if (leaf_abb == abb) { @@ -203,7 +203,7 @@ void item_remove(BVHHandle p_handle) { } // returns success -bool item_activate(BVHHandle p_handle, const AABB &p_aabb) { +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()) { @@ -212,7 +212,7 @@ bool item_activate(BVHHandle p_handle, const AABB &p_aabb) { } // add to tree - BVH_ABB abb; + BVHABB_CLASS abb; abb.from(p_aabb); _current_tree = _handle_get_tree_id(p_handle); @@ -236,7 +236,7 @@ bool item_deactivate(BVHHandle p_handle) { } // remove from tree - BVH_ABB abb; + BVHABB_CLASS abb; node_remove_item(ref_id, &abb); // mark as inactive @@ -269,7 +269,7 @@ bool item_is_pairable(const BVHHandle &p_handle) { return extra.pairable != 0; } -void item_get_ABB(const BVHHandle &p_handle, BVH_ABB &r_abb) { +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]; @@ -297,7 +297,7 @@ void item_set_pairable(const BVHHandle &p_handle, bool p_pairable, uint32_t p_pa // record abb TNode &tnode = _nodes[ref.tnode_id]; TLeaf &leaf = _node_get_leaf(tnode); - BVH_ABB abb = leaf.get_aabb(ref.item_id); + BVHABB_CLASS abb = leaf.get_aabb(ref.item_id); // make sure current tree is correct prior to changing _current_tree = _handle_get_tree_id(p_handle); @@ -377,7 +377,7 @@ void update() { //#define BVH_ALLOW_AUTO_EXPANSION #ifdef BVH_ALLOW_AUTO_EXPANSION if (_auto_node_expansion || _auto_pairing_expansion) { - BVH_ABB world_bound; + BVHABB_CLASS world_bound; world_bound.set_to_max_opposite_extents(); bool bound_valid = false; @@ -392,7 +392,7 @@ void update() { // if there are no nodes, do nothing, but if there are... if (bound_valid) { - AABB bb; + Bounds bb; world_bound.to(bb); real_t size = bb.get_longest_axis_size(); diff --git a/core/math/bvh_refit.inc b/core/math/bvh_refit.inc index b16bb0ab85e..46894303a6b 100644 --- a/core/math/bvh_refit.inc +++ b/core/math/bvh_refit.inc @@ -1,10 +1,10 @@ void _debug_node_verify_bound(uint32_t p_node_id) { TNode &node = _nodes[p_node_id]; - BVH_ABB abb_before = node.aabb; + BVHABB_CLASS abb_before = node.aabb; node_update_aabb(node); - BVH_ABB abb_after = node.aabb; + BVHABB_CLASS abb_after = node.aabb; CRASH_COND(abb_before != abb_after); } diff --git a/core/math/bvh_split.inc b/core/math/bvh_split.inc index e6a63d24d70..43a8b9a72b9 100644 --- a/core/math/bvh_split.inc +++ b/core/math/bvh_split.inc @@ -11,7 +11,7 @@ void _split_inform_references(uint32_t p_node_id) { } } -void _split_leaf_sort_groups_simple(int &num_a, int &num_b, uint16_t *group_a, uint16_t *group_b, const BVH_ABB *temp_bounds, const BVH_ABB full_bound) { +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 (MAX_ITEMS < 4) { uint32_t ind = group_a[0]; @@ -25,8 +25,8 @@ void _split_leaf_sort_groups_simple(int &num_a, int &num_b, uint16_t *group_a, u return; } - Vector3 centre = full_bound.calculate_centre(); - Vector3 size = full_bound.calculate_size(); + Point centre = full_bound.calculate_centre(); + Point size = full_bound.calculate_size(); int order[3]; @@ -135,16 +135,16 @@ void _split_leaf_sort_groups_simple(int &num_a, int &num_b, uint16_t *group_a, u } } -void _split_leaf_sort_groups(int &num_a, int &num_b, uint16_t *group_a, uint16_t *group_b, const BVH_ABB *temp_bounds) { - BVH_ABB groupb_aabb; +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]); } - BVH_ABB groupb_aabb_new; + BVHABB_CLASS groupb_aabb_new; - BVH_ABB rest_aabb; + BVHABB_CLASS rest_aabb; float best_size = FLT_MAX; int best_candidate = -1; @@ -181,12 +181,12 @@ void _split_leaf_sort_groups(int &num_a, int &num_b, uint16_t *group_a, uint16_t group_a[best_candidate] = group_a[num_a]; } -uint32_t split_leaf(uint32_t p_node_id, const BVH_ABB &p_added_item_aabb) { +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 BVH_ABB &p_added_item_aabb) { +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 @@ -231,7 +231,7 @@ uint32_t split_leaf_complex(uint32_t p_node_id, const BVH_ABB &p_added_item_aabb 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... - BVH_ABB *temp_bounds = (BVH_ABB *)alloca(sizeof(BVH_ABB) * max_children); + BVHABB_CLASS *temp_bounds = (BVHABB_CLASS *)alloca(sizeof(BVHABB_CLASS) * max_children); int num_a = max_children; int num_b = 0; @@ -257,7 +257,7 @@ uint32_t split_leaf_complex(uint32_t p_node_id, const BVH_ABB &p_added_item_aabb int which = group_a[n]; if (which != wildcard) { - const BVH_ABB &source_item_aabb = orig_leaf.get_aabb(which); + 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); @@ -269,7 +269,7 @@ uint32_t split_leaf_complex(uint32_t p_node_id, const BVH_ABB &p_added_item_aabb int which = group_b[n]; if (which != wildcard) { - const BVH_ABB &source_item_aabb = orig_leaf.get_aabb(which); + 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); diff --git a/core/math/bvh_structs.inc b/core/math/bvh_structs.inc index 26264f31f4a..ca9acd36656 100644 --- a/core/math/bvh_structs.inc +++ b/core/math/bvh_structs.inc @@ -31,7 +31,7 @@ struct ItemExtra { // this is an item OR a child node depending on whether a leaf node struct Item { - BVH_ABB aabb; + BVHABB_CLASS aabb; uint32_t item_ref_id; }; @@ -43,12 +43,12 @@ private: uint16_t dirty; // separate data orientated lists for faster SIMD traversal uint32_t item_ref_ids[MAX_ITEMS]; - BVH_ABB aabbs[MAX_ITEMS]; + BVHABB_CLASS aabbs[MAX_ITEMS]; public: // accessors - BVH_ABB &get_aabb(uint32_t p_id) { return aabbs[p_id]; } - const BVH_ABB &get_aabb(uint32_t p_id) const { return aabbs[p_id]; } + BVHABB_CLASS &get_aabb(uint32_t p_id) { return aabbs[p_id]; } + const BVHABB_CLASS &get_aabb(uint32_t p_id) const { return aabbs[p_id]; } uint32_t &get_item_ref_id(uint32_t p_id) { return item_ref_ids[p_id]; } const uint32_t &get_item_ref_id(uint32_t p_id) const { return item_ref_ids[p_id]; } @@ -81,7 +81,7 @@ public: // tree node struct TNode { - BVH_ABB aabb; + BVHABB_CLASS aabb; // either number of children if positive // or leaf id if negative (leaf id 0 is disallowed) union { diff --git a/core/math/bvh_tree.h b/core/math/bvh_tree.h index a6dc2a6817d..818612632d3 100644 --- a/core/math/bvh_tree.h +++ b/core/math/bvh_tree.h @@ -48,6 +48,8 @@ #include "core/print_string.h" #include +#define BVHABB_CLASS BVH_ABB + // never do these checks in release #if defined(TOOLS_ENABLED) && defined(DEBUG_ENABLED) //#define BVH_VERBOSE @@ -146,7 +148,7 @@ public: } }; -template +template class BVH_Tree { friend class BVH; @@ -269,7 +271,7 @@ private: node.neg_leaf_id = -(int)child_leaf_id; } - void node_remove_item(uint32_t p_ref_id, BVH_ABB *r_old_aabb = nullptr) { + void node_remove_item(uint32_t p_ref_id, BVHABB_CLASS *r_old_aabb = nullptr) { // get the reference ItemRef &ref = _refs[p_ref_id]; uint32_t owner_node_id = ref.tnode_id; @@ -286,7 +288,7 @@ private: // 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 BVH_ABB &old_aabb = leaf.get_aabb(ref.item_id); + 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 @@ -294,7 +296,7 @@ private: // 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. - BVH_ABB node_bound = tnode.aabb; + BVHABB_CLASS node_bound = tnode.aabb; node_bound.expand(-_node_expansion - 0.001f); bool refit = true; @@ -348,7 +350,7 @@ private: // 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 BVH_ABB &p_aabb) { + 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; @@ -362,7 +364,7 @@ private: bool needs_refit = true; // expand bound now - BVH_ABB expanded = p_aabb; + BVHABB_CLASS expanded = p_aabb; expanded.expand(_node_expansion); // the bound will only be valid if there is an item in there already @@ -390,7 +392,7 @@ private: return needs_refit; } - uint32_t _node_create_another_child(uint32_t p_node_id, const BVH_ABB &p_aabb) { + 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(); diff --git a/core/math/rect2.h b/core/math/rect2.h index fd76f26191d..ea8aeda8a8c 100644 --- a/core/math/rect2.h +++ b/core/math/rect2.h @@ -170,15 +170,18 @@ struct Rect2 { bool operator!=(const Rect2 &p_rect) const { return position != p_rect.position || size != p_rect.size; } inline Rect2 grow(real_t p_by) const { - Rect2 g = *this; - g.position.x -= p_by; - g.position.y -= p_by; - g.size.width += p_by * 2; - g.size.height += p_by * 2; + g.grow_by(p_by); return g; } + inline void grow_by(real_t p_by) { + position.x -= p_by; + position.y -= p_by; + size.width += p_by * 2; + size.height += p_by * 2; + } + inline Rect2 grow_margin(Margin p_margin, real_t p_amount) const { Rect2 g = *this; g = g.grow_individual((MARGIN_LEFT == p_margin) ? p_amount : 0, diff --git a/core/math/vector2.h b/core/math/vector2.h index 4190f0f5b11..0f78700149b 100644 --- a/core/math/vector2.h +++ b/core/math/vector2.h @@ -37,6 +37,7 @@ struct Vector2i; struct Vector2 { + static const int AXIS_COUNT = 2; enum Axis { AXIS_X, @@ -44,12 +45,18 @@ struct Vector2 { }; union { - real_t x; - real_t width; - }; - union { - real_t y; - real_t height; + struct { + union { + real_t x; + real_t width; + }; + union { + real_t y; + real_t height; + }; + }; + + real_t coord[2]; }; _FORCE_INLINE_ real_t &operator[](int p_idx) { @@ -59,6 +66,18 @@ struct Vector2 { return p_idx ? y : x; } + _FORCE_INLINE_ void set_all(real_t p_value) { + x = y = p_value; + } + + _FORCE_INLINE_ int min_axis() const { + return x < y ? 0 : 1; + } + + _FORCE_INLINE_ int max_axis() const { + return x < y ? 1 : 0; + } + void normalize(); Vector2 normalized() const; bool is_normalized() const; diff --git a/core/math/vector3.cpp b/core/math/vector3.cpp index 37aa7d23e26..d2a56510760 100644 --- a/core/math/vector3.cpp +++ b/core/math/vector3.cpp @@ -54,15 +54,6 @@ real_t Vector3::get_axis(int p_axis) const { return operator[](p_axis); } -int Vector3::min_axis() const { - - return x < y ? (x < z ? 0 : 2) : (y < z ? 1 : 2); -} -int Vector3::max_axis() const { - - return x < y ? (y < z ? 2 : 1) : (x < z ? 2 : 0); -} - void Vector3::snap(Vector3 p_val) { x = Math::stepify(x, p_val.x); diff --git a/core/math/vector3.h b/core/math/vector3.h index fdd3a0087f9..acd7b6968c5 100644 --- a/core/math/vector3.h +++ b/core/math/vector3.h @@ -37,6 +37,7 @@ class Basis; struct Vector3 { + static const int AXIS_COUNT = 3; enum Axis { AXIS_X, @@ -67,8 +68,17 @@ struct Vector3 { void set_axis(int p_axis, real_t p_value); real_t get_axis(int p_axis) const; - int min_axis() const; - int max_axis() const; + _FORCE_INLINE_ void set_all(real_t p_value) { + x = y = z = p_value; + } + + _FORCE_INLINE_ int min_axis() const { + return x < y ? (x < z ? 0 : 2) : (y < z ? 1 : 2); + } + + _FORCE_INLINE_ int max_axis() const { + return x < y ? (y < z ? 2 : 1) : (x < z ? 2 : 0); + } _FORCE_INLINE_ real_t length() const; _FORCE_INLINE_ real_t length_squared() const; diff --git a/doc/classes/ProjectSettings.xml b/doc/classes/ProjectSettings.xml index 13148f25be6..cc0c9499513 100644 --- a/doc/classes/ProjectSettings.xml +++ b/doc/classes/ProjectSettings.xml @@ -935,9 +935,11 @@ Size of the hash table used for the broad-phase 2D hash grid algorithm. + [b]Note:[/b] Not used if [member ProjectSettings.physics/2d/use_bvh] is enabled. Cell size used for the broad-phase 2D hash grid algorithm (in pixels). + [b]Note:[/b] Not used if [member ProjectSettings.physics/2d/use_bvh] is enabled. The default angular damp in 2D. @@ -965,6 +967,7 @@ Threshold defining the surface size that constitutes a large object with regard to cells in the broad-phase 2D hash grid algorithm. + [b]Note:[/b] Not used if [member ProjectSettings.physics/2d/use_bvh] is enabled. Sets which physics engine to use for 2D physics. @@ -983,6 +986,9 @@ Time (in seconds) of inactivity before which a 2D physics body will put to sleep. See [constant Physics2DServer.SPACE_PARAM_BODY_TIME_TO_SLEEP]. + + Enables the use of bounding volume hierarchy instead of hash grid for 2D physics spatial partitioning. This may give better performance. + Sets whether the 3D physics world will be created with support for [SoftBody] physics. Only applies to the Bullet physics engine. @@ -1011,7 +1017,7 @@ [b]Note:[/b] Good values are in the range [code]0[/code] to [code]1[/code]. At value [code]0[/code] objects will keep moving with the same velocity. Values greater than [code]1[/code] will aim to reduce the velocity to [code]0[/code] in less than a second e.g. a value of [code]2[/code] will aim to reduce the velocity to [code]0[/code] in half a second. A value equal to or greater than the physics frame rate ([member ProjectSettings.physics/common/physics_fps], [code]60[/code] by default) will bring the object to a stop in one iteration. - Enables the use of bounding volume hierarchy instead of octree for physics spatial partitioning. This may give better performance. + Enables the use of bounding volume hierarchy instead of octree for 3D physics spatial partitioning. This may give better performance. Sets which physics engine to use for 3D physics. diff --git a/servers/physics_2d/broad_phase_2d_basic.cpp b/servers/physics_2d/broad_phase_2d_basic.cpp index 49dba3f565e..de66686df89 100644 --- a/servers/physics_2d/broad_phase_2d_basic.cpp +++ b/servers/physics_2d/broad_phase_2d_basic.cpp @@ -30,7 +30,7 @@ #include "broad_phase_2d_basic.h" -BroadPhase2DBasic::ID BroadPhase2DBasic::create(CollisionObject2DSW *p_object_, int p_subindex) { +BroadPhase2DBasic::ID BroadPhase2DBasic::create(CollisionObject2DSW *p_object_, int p_subindex, const Rect2 &p_aabb, bool p_static) { current++; diff --git a/servers/physics_2d/broad_phase_2d_basic.h b/servers/physics_2d/broad_phase_2d_basic.h index 099b4c1a058..5203b29412e 100644 --- a/servers/physics_2d/broad_phase_2d_basic.h +++ b/servers/physics_2d/broad_phase_2d_basic.h @@ -82,7 +82,7 @@ class BroadPhase2DBasic : public BroadPhase2DSW { public: // 0 is an invalid ID - virtual ID create(CollisionObject2DSW *p_object_, int p_subindex = 0); + virtual ID create(CollisionObject2DSW *p_object_, int p_subindex = 0, const Rect2 &p_aabb = Rect2(), bool p_static = false); virtual void move(ID p_id, const Rect2 &p_aabb); virtual void set_static(ID p_id, bool p_static); virtual void remove(ID p_id); diff --git a/servers/physics_2d/broad_phase_2d_bvh.cpp b/servers/physics_2d/broad_phase_2d_bvh.cpp new file mode 100644 index 00000000000..048d142d4a0 --- /dev/null +++ b/servers/physics_2d/broad_phase_2d_bvh.cpp @@ -0,0 +1,126 @@ +/*************************************************************************/ +/* broad_phase_2d_bvh.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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. */ +/*************************************************************************/ + +#include "broad_phase_2d_bvh.h" +#include "collision_object_2d_sw.h" +#include "core/project_settings.h" + +BroadPhase2DSW::ID BroadPhase2DBVH::create(CollisionObject2DSW *p_object, int p_subindex, const Rect2 &p_aabb, bool p_static) { + + ID oid = bvh.create(p_object, true, p_aabb, p_subindex, !p_static, 1 << p_object->get_type(), p_static ? 0 : 0xFFFFF); // Pair everything, don't care? + return oid + 1; +} + +void BroadPhase2DBVH::move(ID p_id, const Rect2 &p_aabb) { + + bvh.move(p_id - 1, p_aabb); +} + +void BroadPhase2DBVH::set_static(ID p_id, bool p_static) { + + CollisionObject2DSW *it = bvh.get(p_id - 1); + bvh.set_pairable(p_id - 1, !p_static, 1 << it->get_type(), p_static ? 0 : 0xFFFFF); // Pair everything, don't care? +} +void BroadPhase2DBVH::remove(ID p_id) { + + bvh.erase(p_id - 1); +} + +CollisionObject2DSW *BroadPhase2DBVH::get_object(ID p_id) const { + + CollisionObject2DSW *it = bvh.get(p_id - 1); + ERR_FAIL_COND_V(!it, NULL); + return it; +} +bool BroadPhase2DBVH::is_static(ID p_id) const { + + return !bvh.is_pairable(p_id - 1); +} +int BroadPhase2DBVH::get_subindex(ID p_id) const { + + return bvh.get_subindex(p_id - 1); +} + +int BroadPhase2DBVH::cull_segment(const Vector2 &p_from, const Vector2 &p_to, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices) { + + return bvh.cull_segment(p_from, p_to, p_results, p_max_results, p_result_indices); +} + +int BroadPhase2DBVH::cull_aabb(const Rect2 &p_aabb, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices) { + + return bvh.cull_aabb(p_aabb, p_results, p_max_results, p_result_indices); +} + +void *BroadPhase2DBVH::_pair_callback(void *self, uint32_t p_A, CollisionObject2DSW *p_object_A, int subindex_A, uint32_t p_B, CollisionObject2DSW *p_object_B, int subindex_B) { + + BroadPhase2DBVH *bpo = (BroadPhase2DBVH *)(self); + if (!bpo->pair_callback) + return NULL; + + return bpo->pair_callback(p_object_A, subindex_A, p_object_B, subindex_B, bpo->pair_userdata); +} + +void BroadPhase2DBVH::_unpair_callback(void *self, uint32_t p_A, CollisionObject2DSW *p_object_A, int subindex_A, uint32_t p_B, CollisionObject2DSW *p_object_B, int subindex_B, void *pairdata) { + + BroadPhase2DBVH *bpo = (BroadPhase2DBVH *)(self); + if (!bpo->unpair_callback) + return; + + bpo->unpair_callback(p_object_A, subindex_A, p_object_B, subindex_B, pairdata, bpo->unpair_userdata); +} + +void BroadPhase2DBVH::set_pair_callback(PairCallback p_pair_callback, void *p_userdata) { + + pair_callback = p_pair_callback; + pair_userdata = p_userdata; +} + +void BroadPhase2DBVH::set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) { + + unpair_callback = p_unpair_callback; + unpair_userdata = p_userdata; +} + +void BroadPhase2DBVH::update() { + bvh.update(); +} + +BroadPhase2DSW *BroadPhase2DBVH::_create() { + + return memnew(BroadPhase2DBVH); +} + +BroadPhase2DBVH::BroadPhase2DBVH() { + bvh.set_pair_callback(_pair_callback, this); + bvh.set_unpair_callback(_unpair_callback, this); + pair_callback = NULL; + pair_userdata = NULL; + unpair_userdata = NULL; +} diff --git a/servers/physics_2d/broad_phase_2d_bvh.h b/servers/physics_2d/broad_phase_2d_bvh.h new file mode 100644 index 00000000000..9a8c4dc85b0 --- /dev/null +++ b/servers/physics_2d/broad_phase_2d_bvh.h @@ -0,0 +1,74 @@ +/*************************************************************************/ +/* broad_phase_2d_bvh.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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 BROAD_PHASE_2D_BVH_H +#define BROAD_PHASE_2D_BVH_H + +#include "broad_phase_2d_sw.h" +#include "core/math/bvh.h" +#include "core/math/rect2.h" +#include "core/math/vector2.h" + +class BroadPhase2DBVH : public BroadPhase2DSW { + + BVH_Manager bvh; + + static void *_pair_callback(void *, uint32_t, CollisionObject2DSW *, int, uint32_t, CollisionObject2DSW *, int); + static void _unpair_callback(void *, uint32_t, CollisionObject2DSW *, int, uint32_t, CollisionObject2DSW *, int, void *); + + PairCallback pair_callback; + void *pair_userdata; + UnpairCallback unpair_callback; + void *unpair_userdata; + +public: + // 0 is an invalid ID + virtual ID create(CollisionObject2DSW *p_object, int p_subindex = 0, const Rect2 &p_aabb = Rect2(), bool p_static = false); + virtual void move(ID p_id, const Rect2 &p_aabb); + virtual void set_static(ID p_id, bool p_static); + virtual void remove(ID p_id); + + virtual CollisionObject2DSW *get_object(ID p_id) const; + virtual bool is_static(ID p_id) const; + virtual int get_subindex(ID p_id) const; + + virtual int cull_segment(const Vector2 &p_from, const Vector2 &p_to, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices = NULL); + virtual int cull_aabb(const Rect2 &p_aabb, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices = NULL); + + virtual void set_pair_callback(PairCallback p_pair_callback, void *p_userdata); + virtual void set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata); + + virtual void update(); + + static BroadPhase2DSW *_create(); + BroadPhase2DBVH(); +}; + +#endif // BROAD_PHASE_2D_BVH_H diff --git a/servers/physics_2d/broad_phase_2d_hash_grid.cpp b/servers/physics_2d/broad_phase_2d_hash_grid.cpp index be73d908dee..537e173daa1 100644 --- a/servers/physics_2d/broad_phase_2d_hash_grid.cpp +++ b/servers/physics_2d/broad_phase_2d_hash_grid.cpp @@ -317,7 +317,7 @@ void BroadPhase2DHashGrid::_exit_grid(Element *p_elem, const Rect2 &p_rect, bool } } -BroadPhase2DHashGrid::ID BroadPhase2DHashGrid::create(CollisionObject2DSW *p_object, int p_subindex) { +BroadPhase2DHashGrid::ID BroadPhase2DHashGrid::create(CollisionObject2DSW *p_object, int p_subindex, const Rect2 &p_aabb, bool p_static) { current++; @@ -633,15 +633,15 @@ BroadPhase2DSW *BroadPhase2DHashGrid::_create() { BroadPhase2DHashGrid::BroadPhase2DHashGrid() { - hash_table_size = GLOBAL_DEF("physics/2d/bp_hash_table_size", 4096); + hash_table_size = GLOBAL_GET("physics/2d/bp_hash_table_size"); ProjectSettings::get_singleton()->set_custom_property_info("physics/2d/bp_hash_table_size", PropertyInfo(Variant::INT, "physics/2d/bp_hash_table_size", PROPERTY_HINT_RANGE, "0,8192,1,or_greater")); hash_table_size = Math::larger_prime(hash_table_size); hash_table = memnew_arr(PosBin *, hash_table_size); - cell_size = GLOBAL_DEF("physics/2d/cell_size", 128); + cell_size = GLOBAL_GET("physics/2d/cell_size"); ProjectSettings::get_singleton()->set_custom_property_info("physics/2d/cell_size", PropertyInfo(Variant::INT, "physics/2d/cell_size", PROPERTY_HINT_RANGE, "0,512,1,or_greater")); - large_object_min_surface = GLOBAL_DEF("physics/2d/large_object_surface_threshold_in_cells", 512); + large_object_min_surface = GLOBAL_GET("physics/2d/large_object_surface_threshold_in_cells"); ProjectSettings::get_singleton()->set_custom_property_info("physics/2d/large_object_surface_threshold_in_cells", PropertyInfo(Variant::INT, "physics/2d/large_object_surface_threshold_in_cells", PROPERTY_HINT_RANGE, "0,1024,1,or_greater")); for (uint32_t i = 0; i < hash_table_size; i++) diff --git a/servers/physics_2d/broad_phase_2d_hash_grid.h b/servers/physics_2d/broad_phase_2d_hash_grid.h index 29e9ccee0a1..c065d833073 100644 --- a/servers/physics_2d/broad_phase_2d_hash_grid.h +++ b/servers/physics_2d/broad_phase_2d_hash_grid.h @@ -168,7 +168,7 @@ class BroadPhase2DHashGrid : public BroadPhase2DSW { void _check_motion(Element *p_elem); public: - virtual ID create(CollisionObject2DSW *p_object, int p_subindex = 0); + virtual ID create(CollisionObject2DSW *p_object, int p_subindex = 0, const Rect2 &p_aabb = Rect2(), bool p_static = false); virtual void move(ID p_id, const Rect2 &p_aabb); virtual void set_static(ID p_id, bool p_static); virtual void remove(ID p_id); diff --git a/servers/physics_2d/broad_phase_2d_sw.h b/servers/physics_2d/broad_phase_2d_sw.h index 12f1fd680ea..29e898c54a7 100644 --- a/servers/physics_2d/broad_phase_2d_sw.h +++ b/servers/physics_2d/broad_phase_2d_sw.h @@ -49,7 +49,7 @@ public: typedef void (*UnpairCallback)(CollisionObject2DSW *A, int p_subindex_A, CollisionObject2DSW *B, int p_subindex_B, void *p_data, void *p_userdata); // 0 is an invalid ID - virtual ID create(CollisionObject2DSW *p_object_, int p_subindex = 0) = 0; + virtual ID create(CollisionObject2DSW *p_object_, int p_subindex = 0, const Rect2 &p_aabb = Rect2(), bool p_static = false) = 0; virtual void move(ID p_id, const Rect2 &p_aabb) = 0; virtual void set_static(ID p_id, bool p_static) = 0; virtual void remove(ID p_id) = 0; diff --git a/servers/physics_2d/collision_object_2d_sw.cpp b/servers/physics_2d/collision_object_2d_sw.cpp index 08a641078ad..7453c1baf82 100644 --- a/servers/physics_2d/collision_object_2d_sw.cpp +++ b/servers/physics_2d/collision_object_2d_sw.cpp @@ -187,19 +187,19 @@ void CollisionObject2DSW::_update_shapes() { if (s.disabled) continue; - if (s.bpid == 0) { - s.bpid = space->get_broadphase()->create(this, i); - space->get_broadphase()->set_static(s.bpid, _static); - } - //not quite correct, should compute the next matrix.. Rect2 shape_aabb = s.shape->get_aabb(); Transform2D xform = transform * s.xform; shape_aabb = xform.xform(shape_aabb); + shape_aabb.grow_by((s.aabb_cache.size.x + s.aabb_cache.size.y) * 0.5 * 0.05); s.aabb_cache = shape_aabb; - s.aabb_cache = s.aabb_cache.grow((s.aabb_cache.size.x + s.aabb_cache.size.y) * 0.5 * 0.05); - space->get_broadphase()->move(s.bpid, s.aabb_cache); + if (s.bpid == 0) { + s.bpid = space->get_broadphase()->create(this, i, shape_aabb, _static); + space->get_broadphase()->set_static(s.bpid, _static); + } + + space->get_broadphase()->move(s.bpid, shape_aabb); } } @@ -214,11 +214,6 @@ void CollisionObject2DSW::_update_shapes_with_motion(const Vector2 &p_motion) { if (s.disabled) continue; - if (s.bpid == 0) { - s.bpid = space->get_broadphase()->create(this, i); - space->get_broadphase()->set_static(s.bpid, _static); - } - //not quite correct, should compute the next matrix.. Rect2 shape_aabb = s.shape->get_aabb(); Transform2D xform = transform * s.xform; @@ -226,6 +221,11 @@ void CollisionObject2DSW::_update_shapes_with_motion(const Vector2 &p_motion) { shape_aabb = shape_aabb.merge(Rect2(shape_aabb.position + p_motion, shape_aabb.size)); //use motion s.aabb_cache = shape_aabb; + if (s.bpid == 0) { + s.bpid = space->get_broadphase()->create(this, i, shape_aabb, _static); + space->get_broadphase()->set_static(s.bpid, _static); + } + space->get_broadphase()->move(s.bpid, shape_aabb); } } diff --git a/servers/physics_2d/physics_2d_server_sw.cpp b/servers/physics_2d/physics_2d_server_sw.cpp index dd1e78d094e..2e007d06c86 100644 --- a/servers/physics_2d/physics_2d_server_sw.cpp +++ b/servers/physics_2d/physics_2d_server_sw.cpp @@ -30,6 +30,7 @@ #include "physics_2d_server_sw.h" #include "broad_phase_2d_basic.h" +#include "broad_phase_2d_bvh.h" #include "broad_phase_2d_hash_grid.h" #include "collision_solver_2d_sw.h" #include "core/os/os.h" @@ -1440,8 +1441,19 @@ Physics2DServerSW *Physics2DServerSW::singletonsw = NULL; Physics2DServerSW::Physics2DServerSW() { singletonsw = this; - BroadPhase2DSW::create_func = BroadPhase2DHashGrid::_create; - //BroadPhase2DSW::create_func=BroadPhase2DBasic::_create; + + GLOBAL_DEF("physics/2d/use_bvh", true); + GLOBAL_DEF("physics/2d/bp_hash_table_size", 4096); + GLOBAL_DEF("physics/2d/cell_size", 128); + GLOBAL_DEF("physics/2d/large_object_surface_threshold_in_cells", 512); + + bool use_bvh = GLOBAL_GET("physics/2d/use_bvh"); + + if (use_bvh) { + BroadPhase2DSW::create_func = BroadPhase2DBVH::_create; + } else { + BroadPhase2DSW::create_func = BroadPhase2DHashGrid::_create; + } active = true; island_count = 0; @@ -1450,7 +1462,7 @@ Physics2DServerSW::Physics2DServerSW() { #ifdef NO_THREADS using_threads = false; #else - using_threads = int(ProjectSettings::get_singleton()->get("physics/2d/thread_model")) == 2; + using_threads = int(GLOBAL_GET("physics/2d/thread_model")) == 2; #endif flushing_queries = false; };