summaryrefslogtreecommitdiff
path: root/servers/physics_3d
diff options
context:
space:
mode:
Diffstat (limited to 'servers/physics_3d')
-rw-r--r--servers/physics_3d/body_3d_sw.cpp23
-rw-r--r--servers/physics_3d/broad_phase_3d_basic.cpp212
-rw-r--r--servers/physics_3d/broad_phase_3d_basic.h105
-rw-r--r--servers/physics_3d/broad_phase_3d_bvh.cpp (renamed from servers/physics_3d/broad_phase_octree.cpp)74
-rw-r--r--servers/physics_3d/broad_phase_3d_bvh.h (renamed from servers/physics_3d/broad_phase_octree.h)22
-rw-r--r--servers/physics_3d/broad_phase_3d_sw.h2
-rw-r--r--servers/physics_3d/collision_object_3d_sw.cpp24
-rw-r--r--servers/physics_3d/collision_solver_3d_sat.cpp2
-rw-r--r--servers/physics_3d/physics_server_3d_sw.cpp6
-rw-r--r--servers/physics_3d/shape_3d_sw.cpp6
10 files changed, 83 insertions, 393 deletions
diff --git a/servers/physics_3d/body_3d_sw.cpp b/servers/physics_3d/body_3d_sw.cpp
index 4357c474e4..e4f9548a61 100644
--- a/servers/physics_3d/body_3d_sw.cpp
+++ b/servers/physics_3d/body_3d_sw.cpp
@@ -65,16 +65,18 @@ void Body3DSW::update_inertias() {
// We have to recompute the center of mass.
center_of_mass_local.zero();
- for (int i = 0; i < get_shape_count(); i++) {
- real_t area = get_shape_area(i);
+ if (total_area != 0.0) {
+ for (int i = 0; i < get_shape_count(); i++) {
+ real_t area = get_shape_area(i);
- real_t mass = area * this->mass / total_area;
+ real_t mass = area * this->mass / total_area;
- // NOTE: we assume that the shape origin is also its center of mass.
- center_of_mass_local += mass * get_shape_transform(i).origin;
- }
+ // NOTE: we assume that the shape origin is also its center of mass.
+ center_of_mass_local += mass * get_shape_transform(i).origin;
+ }
- center_of_mass_local /= mass;
+ center_of_mass_local /= mass;
+ }
// Recompute the inertia tensor.
Basis inertia_tensor;
@@ -86,12 +88,15 @@ void Body3DSW::update_inertias() {
continue;
}
+ real_t area = get_shape_area(i);
+ if (area == 0.0) {
+ continue;
+ }
+
inertia_set = true;
const Shape3DSW *shape = get_shape(i);
- real_t area = get_shape_area(i);
-
real_t mass = area * this->mass / total_area;
Basis shape_inertia_tensor = shape->get_moment_of_inertia(mass).to_diagonal_matrix();
diff --git a/servers/physics_3d/broad_phase_3d_basic.cpp b/servers/physics_3d/broad_phase_3d_basic.cpp
deleted file mode 100644
index b41c2530da..0000000000
--- a/servers/physics_3d/broad_phase_3d_basic.cpp
+++ /dev/null
@@ -1,212 +0,0 @@
-/*************************************************************************/
-/* broad_phase_3d_basic.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_3d_basic.h"
-#include "core/string/print_string.h"
-#include "core/templates/list.h"
-
-BroadPhase3DSW::ID BroadPhase3DBasic::create(CollisionObject3DSW *p_object, int p_subindex) {
- ERR_FAIL_COND_V(p_object == nullptr, 0);
-
- current++;
-
- Element e;
- e.owner = p_object;
- e._static = false;
- e.subindex = p_subindex;
-
- element_map[current] = e;
- return current;
-}
-
-void BroadPhase3DBasic::move(ID p_id, const AABB &p_aabb) {
- Map<ID, Element>::Element *E = element_map.find(p_id);
- ERR_FAIL_COND(!E);
- E->get().aabb = p_aabb;
-}
-
-void BroadPhase3DBasic::set_static(ID p_id, bool p_static) {
- Map<ID, Element>::Element *E = element_map.find(p_id);
- ERR_FAIL_COND(!E);
- E->get()._static = p_static;
-}
-
-void BroadPhase3DBasic::remove(ID p_id) {
- Map<ID, Element>::Element *E = element_map.find(p_id);
- ERR_FAIL_COND(!E);
- List<PairKey> to_erase;
- //unpair must be done immediately on removal to avoid potential invalid pointers
- for (Map<PairKey, void *>::Element *F = pair_map.front(); F; F = F->next()) {
- if (F->key().a == p_id || F->key().b == p_id) {
- if (unpair_callback) {
- Element *elem_A = &element_map[F->key().a];
- Element *elem_B = &element_map[F->key().b];
- unpair_callback(elem_A->owner, elem_A->subindex, elem_B->owner, elem_B->subindex, F->get(), unpair_userdata);
- }
- to_erase.push_back(F->key());
- }
- }
- while (to_erase.size()) {
- pair_map.erase(to_erase.front()->get());
- to_erase.pop_front();
- }
- element_map.erase(E);
-}
-
-CollisionObject3DSW *BroadPhase3DBasic::get_object(ID p_id) const {
- const Map<ID, Element>::Element *E = element_map.find(p_id);
- ERR_FAIL_COND_V(!E, nullptr);
- return E->get().owner;
-}
-
-bool BroadPhase3DBasic::is_static(ID p_id) const {
- const Map<ID, Element>::Element *E = element_map.find(p_id);
- ERR_FAIL_COND_V(!E, false);
- return E->get()._static;
-}
-
-int BroadPhase3DBasic::get_subindex(ID p_id) const {
- const Map<ID, Element>::Element *E = element_map.find(p_id);
- ERR_FAIL_COND_V(!E, -1);
- return E->get().subindex;
-}
-
-int BroadPhase3DBasic::cull_point(const Vector3 &p_point, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices) {
- int rc = 0;
-
- for (Map<ID, Element>::Element *E = element_map.front(); E; E = E->next()) {
- const AABB aabb = E->get().aabb;
- if (aabb.has_point(p_point)) {
- p_results[rc] = E->get().owner;
- p_result_indices[rc] = E->get().subindex;
- rc++;
- if (rc >= p_max_results) {
- break;
- }
- }
- }
-
- return rc;
-}
-
-int BroadPhase3DBasic::cull_segment(const Vector3 &p_from, const Vector3 &p_to, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices) {
- int rc = 0;
-
- for (Map<ID, Element>::Element *E = element_map.front(); E; E = E->next()) {
- const AABB aabb = E->get().aabb;
- if (aabb.intersects_segment(p_from, p_to)) {
- p_results[rc] = E->get().owner;
- p_result_indices[rc] = E->get().subindex;
- rc++;
- if (rc >= p_max_results) {
- break;
- }
- }
- }
-
- return rc;
-}
-
-int BroadPhase3DBasic::cull_aabb(const AABB &p_aabb, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices) {
- int rc = 0;
-
- for (Map<ID, Element>::Element *E = element_map.front(); E; E = E->next()) {
- const AABB aabb = E->get().aabb;
- if (aabb.intersects(p_aabb)) {
- p_results[rc] = E->get().owner;
- p_result_indices[rc] = E->get().subindex;
- rc++;
- if (rc >= p_max_results) {
- break;
- }
- }
- }
-
- return rc;
-}
-
-void BroadPhase3DBasic::set_pair_callback(PairCallback p_pair_callback, void *p_userdata) {
- pair_userdata = p_userdata;
- pair_callback = p_pair_callback;
-}
-
-void BroadPhase3DBasic::set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) {
- unpair_userdata = p_userdata;
- unpair_callback = p_unpair_callback;
-}
-
-void BroadPhase3DBasic::update() {
- // recompute pairs
- for (Map<ID, Element>::Element *I = element_map.front(); I; I = I->next()) {
- for (Map<ID, Element>::Element *J = I->next(); J; J = J->next()) {
- Element *elem_A = &I->get();
- Element *elem_B = &J->get();
-
- if (elem_A->owner == elem_B->owner) {
- continue;
- }
-
- bool pair_ok = elem_A->aabb.intersects(elem_B->aabb) && (!elem_A->_static || !elem_B->_static);
-
- PairKey key(I->key(), J->key());
-
- Map<PairKey, void *>::Element *E = pair_map.find(key);
-
- if (!pair_ok && E) {
- if (unpair_callback) {
- unpair_callback(elem_A->owner, elem_A->subindex, elem_B->owner, elem_B->subindex, E->get(), unpair_userdata);
- }
- pair_map.erase(key);
- }
-
- if (pair_ok && !E) {
- void *data = nullptr;
- if (pair_callback) {
- data = pair_callback(elem_A->owner, elem_A->subindex, elem_B->owner, elem_B->subindex, unpair_userdata);
- if (data) {
- pair_map.insert(key, data);
- }
- }
- }
- }
- }
-}
-
-BroadPhase3DSW *BroadPhase3DBasic::_create() {
- return memnew(BroadPhase3DBasic);
-}
-
-BroadPhase3DBasic::BroadPhase3DBasic() {
- current = 1;
- unpair_callback = nullptr;
- unpair_userdata = nullptr;
- pair_callback = nullptr;
- pair_userdata = nullptr;
-}
diff --git a/servers/physics_3d/broad_phase_3d_basic.h b/servers/physics_3d/broad_phase_3d_basic.h
deleted file mode 100644
index 54d34e005f..0000000000
--- a/servers/physics_3d/broad_phase_3d_basic.h
+++ /dev/null
@@ -1,105 +0,0 @@
-/*************************************************************************/
-/* broad_phase_3d_basic.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_BASIC_H
-#define BROAD_PHASE_BASIC_H
-
-#include "broad_phase_3d_sw.h"
-#include "core/templates/map.h"
-
-class BroadPhase3DBasic : public BroadPhase3DSW {
- struct Element {
- CollisionObject3DSW *owner;
- bool _static;
- AABB aabb;
- int subindex;
- };
-
- Map<ID, Element> element_map;
-
- ID current;
-
- struct PairKey {
- union {
- struct {
- ID a;
- ID b;
- };
- uint64_t key;
- };
-
- _FORCE_INLINE_ bool operator<(const PairKey &p_key) const {
- return key < p_key.key;
- }
-
- PairKey() { key = 0; }
- PairKey(ID p_a, ID p_b) {
- if (p_a > p_b) {
- a = p_b;
- b = p_a;
- } else {
- a = p_a;
- b = p_b;
- }
- }
- };
-
- Map<PairKey, void *> pair_map;
-
- PairCallback pair_callback;
- void *pair_userdata;
- UnpairCallback unpair_callback;
- void *unpair_userdata;
-
-public:
- // 0 is an invalid ID
- virtual ID create(CollisionObject3DSW *p_object, int p_subindex = 0);
- virtual void move(ID p_id, const AABB &p_aabb);
- virtual void set_static(ID p_id, bool p_static);
- virtual void remove(ID p_id);
-
- virtual CollisionObject3DSW *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_point(const Vector3 &p_point, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices = nullptr);
- virtual int cull_segment(const Vector3 &p_from, const Vector3 &p_to, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices = nullptr);
- virtual int cull_aabb(const AABB &p_aabb, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices = nullptr);
-
- 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 BroadPhase3DSW *_create();
- BroadPhase3DBasic();
-};
-
-#endif // BROAD_PHASE_BASIC_H
diff --git a/servers/physics_3d/broad_phase_octree.cpp b/servers/physics_3d/broad_phase_3d_bvh.cpp
index 11324fa4e4..f9f64f786d 100644
--- a/servers/physics_3d/broad_phase_octree.cpp
+++ b/servers/physics_3d/broad_phase_3d_bvh.cpp
@@ -1,5 +1,5 @@
/*************************************************************************/
-/* broad_phase_octree.cpp */
+/* broad_phase_3d_bvh.cpp */
/*************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
@@ -28,55 +28,55 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/
-#include "broad_phase_octree.h"
+#include "broad_phase_3d_bvh.h"
#include "collision_object_3d_sw.h"
-BroadPhase3DSW::ID BroadPhaseOctree::create(CollisionObject3DSW *p_object, int p_subindex) {
- ID oid = octree.create(p_object, AABB(), p_subindex, false, 1 << p_object->get_type(), 0);
- return oid;
+BroadPhase3DBVH::ID BroadPhase3DBVH::create(CollisionObject3DSW *p_object, int p_subindex, const AABB &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 BroadPhaseOctree::move(ID p_id, const AABB &p_aabb) {
- octree.move(p_id, p_aabb);
+void BroadPhase3DBVH::move(ID p_id, const AABB &p_aabb) {
+ bvh.move(p_id - 1, p_aabb);
}
-void BroadPhaseOctree::set_static(ID p_id, bool p_static) {
- CollisionObject3DSW *it = octree.get(p_id);
- octree.set_pairable(p_id, !p_static, 1 << it->get_type(), p_static ? 0 : 0xFFFFF); //pair everything, don't care 1?
+void BroadPhase3DBVH::set_static(ID p_id, bool p_static) {
+ CollisionObject3DSW *it = bvh.get(p_id - 1);
+ bvh.set_pairable(p_id - 1, !p_static, 1 << it->get_type(), p_static ? 0 : 0xFFFFF, false); // Pair everything, don't care?
}
-void BroadPhaseOctree::remove(ID p_id) {
- octree.erase(p_id);
+void BroadPhase3DBVH::remove(ID p_id) {
+ bvh.erase(p_id - 1);
}
-CollisionObject3DSW *BroadPhaseOctree::get_object(ID p_id) const {
- CollisionObject3DSW *it = octree.get(p_id);
+CollisionObject3DSW *BroadPhase3DBVH::get_object(ID p_id) const {
+ CollisionObject3DSW *it = bvh.get(p_id - 1);
ERR_FAIL_COND_V(!it, nullptr);
return it;
}
-bool BroadPhaseOctree::is_static(ID p_id) const {
- return !octree.is_pairable(p_id);
+bool BroadPhase3DBVH::is_static(ID p_id) const {
+ return !bvh.is_pairable(p_id - 1);
}
-int BroadPhaseOctree::get_subindex(ID p_id) const {
- return octree.get_subindex(p_id);
+int BroadPhase3DBVH::get_subindex(ID p_id) const {
+ return bvh.get_subindex(p_id - 1);
}
-int BroadPhaseOctree::cull_point(const Vector3 &p_point, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices) {
- return octree.cull_point(p_point, p_results, p_max_results, p_result_indices);
+int BroadPhase3DBVH::cull_point(const Vector3 &p_point, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices) {
+ return bvh.cull_point(p_point, p_results, p_max_results, p_result_indices);
}
-int BroadPhaseOctree::cull_segment(const Vector3 &p_from, const Vector3 &p_to, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices) {
- return octree.cull_segment(p_from, p_to, p_results, p_max_results, p_result_indices);
+int BroadPhase3DBVH::cull_segment(const Vector3 &p_from, const Vector3 &p_to, CollisionObject3DSW **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 BroadPhaseOctree::cull_aabb(const AABB &p_aabb, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices) {
- return octree.cull_aabb(p_aabb, p_results, p_max_results, p_result_indices);
+int BroadPhase3DBVH::cull_aabb(const AABB &p_aabb, CollisionObject3DSW **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 *BroadPhaseOctree::_pair_callback(void *self, OctreeElementID p_A, CollisionObject3DSW *p_object_A, int subindex_A, OctreeElementID p_B, CollisionObject3DSW *p_object_B, int subindex_B) {
- BroadPhaseOctree *bpo = (BroadPhaseOctree *)(self);
+void *BroadPhase3DBVH::_pair_callback(void *self, uint32_t p_A, CollisionObject3DSW *p_object_A, int subindex_A, uint32_t p_B, CollisionObject3DSW *p_object_B, int subindex_B) {
+ BroadPhase3DBVH *bpo = (BroadPhase3DBVH *)(self);
if (!bpo->pair_callback) {
return nullptr;
}
@@ -84,8 +84,8 @@ void *BroadPhaseOctree::_pair_callback(void *self, OctreeElementID p_A, Collisio
return bpo->pair_callback(p_object_A, subindex_A, p_object_B, subindex_B, bpo->pair_userdata);
}
-void BroadPhaseOctree::_unpair_callback(void *self, OctreeElementID p_A, CollisionObject3DSW *p_object_A, int subindex_A, OctreeElementID p_B, CollisionObject3DSW *p_object_B, int subindex_B, void *pairdata) {
- BroadPhaseOctree *bpo = (BroadPhaseOctree *)(self);
+void BroadPhase3DBVH::_unpair_callback(void *self, uint32_t p_A, CollisionObject3DSW *p_object_A, int subindex_A, uint32_t p_B, CollisionObject3DSW *p_object_B, int subindex_B, void *pairdata) {
+ BroadPhase3DBVH *bpo = (BroadPhase3DBVH *)(self);
if (!bpo->unpair_callback) {
return;
}
@@ -93,27 +93,27 @@ void BroadPhaseOctree::_unpair_callback(void *self, OctreeElementID p_A, Collisi
bpo->unpair_callback(p_object_A, subindex_A, p_object_B, subindex_B, pairdata, bpo->unpair_userdata);
}
-void BroadPhaseOctree::set_pair_callback(PairCallback p_pair_callback, void *p_userdata) {
+void BroadPhase3DBVH::set_pair_callback(PairCallback p_pair_callback, void *p_userdata) {
pair_callback = p_pair_callback;
pair_userdata = p_userdata;
}
-void BroadPhaseOctree::set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) {
+void BroadPhase3DBVH::set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) {
unpair_callback = p_unpair_callback;
unpair_userdata = p_userdata;
}
-void BroadPhaseOctree::update() {
- // does.. not?
+void BroadPhase3DBVH::update() {
+ bvh.update();
}
-BroadPhase3DSW *BroadPhaseOctree::_create() {
- return memnew(BroadPhaseOctree);
+BroadPhase3DSW *BroadPhase3DBVH::_create() {
+ return memnew(BroadPhase3DBVH);
}
-BroadPhaseOctree::BroadPhaseOctree() {
- octree.set_pair_callback(_pair_callback, this);
- octree.set_unpair_callback(_unpair_callback, this);
+BroadPhase3DBVH::BroadPhase3DBVH() {
+ bvh.set_pair_callback(_pair_callback, this);
+ bvh.set_unpair_callback(_unpair_callback, this);
pair_callback = nullptr;
pair_userdata = nullptr;
unpair_userdata = nullptr;
diff --git a/servers/physics_3d/broad_phase_octree.h b/servers/physics_3d/broad_phase_3d_bvh.h
index ee681dda96..30b8b7f2aa 100644
--- a/servers/physics_3d/broad_phase_octree.h
+++ b/servers/physics_3d/broad_phase_3d_bvh.h
@@ -1,5 +1,5 @@
/*************************************************************************/
-/* broad_phase_octree.h */
+/* broad_phase_3d_bvh.h */
/*************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
@@ -28,17 +28,17 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/
-#ifndef BROAD_PHASE_OCTREE_H
-#define BROAD_PHASE_OCTREE_H
+#ifndef BROAD_PHASE_3D_BVH_H
+#define BROAD_PHASE_3D_BVH_H
#include "broad_phase_3d_sw.h"
-#include "core/math/octree.h"
+#include "core/math/bvh.h"
-class BroadPhaseOctree : public BroadPhase3DSW {
- Octree<CollisionObject3DSW, true> octree;
+class BroadPhase3DBVH : public BroadPhase3DSW {
+ BVH_Manager<CollisionObject3DSW, true, 128> bvh;
- static void *_pair_callback(void *, OctreeElementID, CollisionObject3DSW *, int, OctreeElementID, CollisionObject3DSW *, int);
- static void _unpair_callback(void *, OctreeElementID, CollisionObject3DSW *, int, OctreeElementID, CollisionObject3DSW *, int, void *);
+ static void *_pair_callback(void *, uint32_t, CollisionObject3DSW *, int, uint32_t, CollisionObject3DSW *, int);
+ static void _unpair_callback(void *, uint32_t, CollisionObject3DSW *, int, uint32_t, CollisionObject3DSW *, int, void *);
PairCallback pair_callback;
void *pair_userdata;
@@ -47,7 +47,7 @@ class BroadPhaseOctree : public BroadPhase3DSW {
public:
// 0 is an invalid ID
- virtual ID create(CollisionObject3DSW *p_object, int p_subindex = 0);
+ virtual ID create(CollisionObject3DSW *p_object, int p_subindex = 0, const AABB &p_aabb = AABB(), bool p_static = false);
virtual void move(ID p_id, const AABB &p_aabb);
virtual void set_static(ID p_id, bool p_static);
virtual void remove(ID p_id);
@@ -66,7 +66,7 @@ public:
virtual void update();
static BroadPhase3DSW *_create();
- BroadPhaseOctree();
+ BroadPhase3DBVH();
};
-#endif // BROAD_PHASE_OCTREE_H
+#endif // BROAD_PHASE_3D_BVH_H
diff --git a/servers/physics_3d/broad_phase_3d_sw.h b/servers/physics_3d/broad_phase_3d_sw.h
index 283c087b96..98313cb216 100644
--- a/servers/physics_3d/broad_phase_3d_sw.h
+++ b/servers/physics_3d/broad_phase_3d_sw.h
@@ -48,7 +48,7 @@ public:
typedef void (*UnpairCallback)(CollisionObject3DSW *A, int p_subindex_A, CollisionObject3DSW *B, int p_subindex_B, void *p_data, void *p_userdata);
// 0 is an invalid ID
- virtual ID create(CollisionObject3DSW *p_object_, int p_subindex = 0) = 0;
+ virtual ID create(CollisionObject3DSW *p_object_, int p_subindex = 0, const AABB &p_aabb = AABB(), bool p_static = false) = 0;
virtual void move(ID p_id, const AABB &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_3d/collision_object_3d_sw.cpp b/servers/physics_3d/collision_object_3d_sw.cpp
index 293a7e6606..459deb1356 100644
--- a/servers/physics_3d/collision_object_3d_sw.cpp
+++ b/servers/physics_3d/collision_object_3d_sw.cpp
@@ -146,22 +146,23 @@ void CollisionObject3DSW::_update_shapes() {
for (int i = 0; i < shapes.size(); i++) {
Shape &s = shapes.write[i];
- 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..
AABB shape_aabb = s.shape->get_aabb();
Transform 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);
Vector3 scale = xform.get_basis().get_scale();
s.area_cache = s.shape->get_area() * scale.x * scale.y * scale.z;
- 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);
}
}
@@ -172,18 +173,19 @@ void CollisionObject3DSW::_update_shapes_with_motion(const Vector3 &p_motion) {
for (int i = 0; i < shapes.size(); i++) {
Shape &s = shapes.write[i];
- 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..
AABB shape_aabb = s.shape->get_aabb();
Transform xform = transform * s.xform;
shape_aabb = xform.xform(shape_aabb);
- shape_aabb = shape_aabb.merge(AABB(shape_aabb.position + p_motion, shape_aabb.size)); //use motion
+ shape_aabb.merge_with(AABB(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_3d/collision_solver_3d_sat.cpp b/servers/physics_3d/collision_solver_3d_sat.cpp
index 9d5448dbfa..fcac0587b2 100644
--- a/servers/physics_3d/collision_solver_3d_sat.cpp
+++ b/servers/physics_3d/collision_solver_3d_sat.cpp
@@ -690,7 +690,7 @@ public:
Vector3 axis = (p_point_B - p_point_A);
real_t depth = axis.length();
- // Filter out bogus directions with a treshold and re-testing axis.
+ // Filter out bogus directions with a threshold and re-testing axis.
if (separator->best_depth - depth > 0.001) {
separator->test_axis(axis / depth);
}
diff --git a/servers/physics_3d/physics_server_3d_sw.cpp b/servers/physics_3d/physics_server_3d_sw.cpp
index c08e2b5794..f3eb1ae48f 100644
--- a/servers/physics_3d/physics_server_3d_sw.cpp
+++ b/servers/physics_3d/physics_server_3d_sw.cpp
@@ -30,8 +30,7 @@
#include "physics_server_3d_sw.h"
-#include "broad_phase_3d_basic.h"
-#include "broad_phase_octree.h"
+#include "broad_phase_3d_bvh.h"
#include "core/debugger/engine_debugger.h"
#include "core/os/os.h"
#include "joints/cone_twist_joint_3d_sw.h"
@@ -1755,7 +1754,8 @@ void PhysicsServer3DSW::_shape_col_cbk(const Vector3 &p_point_A, int p_index_A,
PhysicsServer3DSW *PhysicsServer3DSW::singletonsw = nullptr;
PhysicsServer3DSW::PhysicsServer3DSW(bool p_using_threads) {
singletonsw = this;
- BroadPhase3DSW::create_func = BroadPhaseOctree::_create;
+ BroadPhase3DSW::create_func = BroadPhase3DBVH::_create;
+
island_count = 0;
active_objects = 0;
collision_pairs = 0;
diff --git a/servers/physics_3d/shape_3d_sw.cpp b/servers/physics_3d/shape_3d_sw.cpp
index ccd37ca742..ca7248993f 100644
--- a/servers/physics_3d/shape_3d_sw.cpp
+++ b/servers/physics_3d/shape_3d_sw.cpp
@@ -31,8 +31,8 @@
#include "shape_3d_sw.h"
#include "core/io/image.h"
+#include "core/math/convex_hull.h"
#include "core/math/geometry_3d.h"
-#include "core/math/quick_hull.h"
#include "core/templates/sort_array.h"
// HeightMapShape3DSW is based on Bullet btHeightfieldTerrainShape.
@@ -1089,9 +1089,9 @@ Vector3 ConvexPolygonShape3DSW::get_moment_of_inertia(real_t p_mass) const {
}
void ConvexPolygonShape3DSW::_setup(const Vector<Vector3> &p_vertices) {
- Error err = QuickHull::build(p_vertices, mesh);
+ Error err = ConvexHullComputer::convex_hull(p_vertices, mesh);
if (err != OK) {
- ERR_PRINT("Failed to build QuickHull");
+ ERR_PRINT("Failed to build convex hull");
}
AABB _aabb;
generated by cgit v1.2.3 (git 2.25.1) at 2025-01-31 22:01:14 +0000