summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--servers/physics_3d/area_3d_sw.cpp8
-rw-r--r--servers/physics_3d/area_3d_sw.h34
-rw-r--r--servers/physics_3d/area_pair_3d_sw.cpp82
-rw-r--r--servers/physics_3d/area_pair_3d_sw.h18
-rw-r--r--servers/physics_3d/body_3d_sw.h12
-rw-r--r--servers/physics_3d/collision_solver_3d_sw.cpp4
-rw-r--r--servers/physics_3d/soft_body_3d_sw.cpp41
-rw-r--r--servers/physics_3d/soft_body_3d_sw.h26
-rw-r--r--servers/physics_3d/space_3d_sw.cpp4
9 files changed, 215 insertions, 14 deletions
diff --git a/servers/physics_3d/area_3d_sw.cpp b/servers/physics_3d/area_3d_sw.cpp
index a9f5c4aec3..364f63e4ad 100644
--- a/servers/physics_3d/area_3d_sw.cpp
+++ b/servers/physics_3d/area_3d_sw.cpp
@@ -30,8 +30,16 @@
#include "area_3d_sw.h"
#include "body_3d_sw.h"
+#include "soft_body_3d_sw.h"
#include "space_3d_sw.h"
+Area3DSW::BodyKey::BodyKey(SoftBody3DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape) {
+ rid = p_body->get_self();
+ instance_id = p_body->get_instance_id();
+ body_shape = p_body_shape;
+ area_shape = p_area_shape;
+}
+
Area3DSW::BodyKey::BodyKey(Body3DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape) {
rid = p_body->get_self();
instance_id = p_body->get_instance_id();
diff --git a/servers/physics_3d/area_3d_sw.h b/servers/physics_3d/area_3d_sw.h
index 12f7545c08..5959ee1e95 100644
--- a/servers/physics_3d/area_3d_sw.h
+++ b/servers/physics_3d/area_3d_sw.h
@@ -38,6 +38,7 @@
class Space3DSW;
class Body3DSW;
+class SoftBody3DSW;
class Constraint3DSW;
class Area3DSW : public CollisionObject3DSW {
@@ -80,6 +81,7 @@ class Area3DSW : public CollisionObject3DSW {
}
_FORCE_INLINE_ BodyKey() {}
+ BodyKey(SoftBody3DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape);
BodyKey(Body3DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape);
BodyKey(Area3DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape);
};
@@ -91,6 +93,7 @@ class Area3DSW : public CollisionObject3DSW {
_FORCE_INLINE_ BodyState() { state = 0; }
};
+ Map<BodyKey, BodyState> monitored_soft_bodies;
Map<BodyKey, BodyState> monitored_bodies;
Map<BodyKey, BodyState> monitored_areas;
@@ -115,6 +118,9 @@ public:
_FORCE_INLINE_ void add_body_to_query(Body3DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape);
_FORCE_INLINE_ void remove_body_from_query(Body3DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape);
+ _FORCE_INLINE_ void add_soft_body_to_query(SoftBody3DSW *p_soft_body, uint32_t p_soft_body_shape, uint32_t p_area_shape);
+ _FORCE_INLINE_ void remove_soft_body_from_query(SoftBody3DSW *p_soft_body, uint32_t p_soft_body_shape, uint32_t p_area_shape);
+
_FORCE_INLINE_ void add_area_to_query(Area3DSW *p_area, uint32_t p_area_shape, uint32_t p_self_shape);
_FORCE_INLINE_ void remove_area_from_query(Area3DSW *p_area, uint32_t p_area_shape, uint32_t p_self_shape);
@@ -166,6 +172,22 @@ public:
~Area3DSW();
};
+void Area3DSW::add_soft_body_to_query(SoftBody3DSW *p_soft_body, uint32_t p_soft_body_shape, uint32_t p_area_shape) {
+ BodyKey bk(p_soft_body, p_soft_body_shape, p_area_shape);
+ monitored_soft_bodies[bk].inc();
+ if (!monitor_query_list.in_list()) {
+ _queue_monitor_update();
+ }
+}
+
+void Area3DSW::remove_soft_body_from_query(SoftBody3DSW *p_soft_body, uint32_t p_soft_body_shape, uint32_t p_area_shape) {
+ BodyKey bk(p_soft_body, p_soft_body_shape, p_area_shape);
+ monitored_soft_bodies[bk].dec();
+ if (!monitor_query_list.in_list()) {
+ _queue_monitor_update();
+ }
+}
+
void Area3DSW::add_body_to_query(Body3DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape) {
BodyKey bk(p_body, p_body_shape, p_area_shape);
monitored_bodies[bk].inc();
@@ -198,4 +220,16 @@ void Area3DSW::remove_area_from_query(Area3DSW *p_area, uint32_t p_area_shape, u
}
}
+struct AreaCMP {
+ Area3DSW *area;
+ int refCount;
+ _FORCE_INLINE_ bool operator==(const AreaCMP &p_cmp) const { return area->get_self() == p_cmp.area->get_self(); }
+ _FORCE_INLINE_ bool operator<(const AreaCMP &p_cmp) const { return area->get_priority() < p_cmp.area->get_priority(); }
+ _FORCE_INLINE_ AreaCMP() {}
+ _FORCE_INLINE_ AreaCMP(Area3DSW *p_area) {
+ area = p_area;
+ refCount = 1;
+ }
+};
+
#endif // AREA__SW_H
diff --git a/servers/physics_3d/area_pair_3d_sw.cpp b/servers/physics_3d/area_pair_3d_sw.cpp
index f43c2c965e..e740565da6 100644
--- a/servers/physics_3d/area_pair_3d_sw.cpp
+++ b/servers/physics_3d/area_pair_3d_sw.cpp
@@ -181,3 +181,85 @@ Area2Pair3DSW::~Area2Pair3DSW() {
area_a->remove_constraint(this);
area_b->remove_constraint(this);
}
+
+////////////////////////////////////////////////////
+
+bool AreaSoftBodyPair3DSW::setup(real_t p_step) {
+ bool result = false;
+ if (
+ area->interacts_with(soft_body) &&
+ CollisionSolver3DSW::solve_static(
+ soft_body->get_shape(soft_body_shape),
+ soft_body->get_transform() * soft_body->get_shape_transform(soft_body_shape),
+ area->get_shape(area_shape),
+ area->get_transform() * area->get_shape_transform(area_shape),
+ nullptr,
+ this)) {
+ result = true;
+ }
+
+ process_collision = false;
+ if (result != colliding) {
+ if (area->get_space_override_mode() != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) {
+ process_collision = true;
+ } else if (area->has_monitor_callback()) {
+ process_collision = true;
+ }
+
+ colliding = result;
+ }
+
+ return process_collision;
+}
+
+bool AreaSoftBodyPair3DSW::pre_solve(real_t p_step) {
+ if (!process_collision) {
+ return false;
+ }
+
+ if (colliding) {
+ if (area->get_space_override_mode() != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) {
+ soft_body->add_area(area);
+ }
+
+ if (area->has_monitor_callback()) {
+ area->add_soft_body_to_query(soft_body, soft_body_shape, area_shape);
+ }
+ } else {
+ if (area->get_space_override_mode() != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) {
+ soft_body->remove_area(area);
+ }
+
+ if (area->has_monitor_callback()) {
+ area->remove_soft_body_from_query(soft_body, soft_body_shape, area_shape);
+ }
+ }
+
+ return false; // Never do any post solving.
+}
+
+void AreaSoftBodyPair3DSW::solve(real_t p_step) {
+ // Nothing to do.
+}
+
+AreaSoftBodyPair3DSW::AreaSoftBodyPair3DSW(SoftBody3DSW *p_soft_body, int p_soft_body_shape, Area3DSW *p_area, int p_area_shape) {
+ soft_body = p_soft_body;
+ area = p_area;
+ soft_body_shape = p_soft_body_shape;
+ area_shape = p_area_shape;
+ soft_body->add_constraint(this);
+ area->add_constraint(this);
+}
+
+AreaSoftBodyPair3DSW::~AreaSoftBodyPair3DSW() {
+ if (colliding) {
+ if (area->get_space_override_mode() != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) {
+ soft_body->remove_area(area);
+ }
+ if (area->has_monitor_callback()) {
+ area->remove_soft_body_from_query(soft_body, soft_body_shape, area_shape);
+ }
+ }
+ soft_body->remove_constraint(this);
+ area->remove_constraint(this);
+}
diff --git a/servers/physics_3d/area_pair_3d_sw.h b/servers/physics_3d/area_pair_3d_sw.h
index 596d893082..8cc9e9ad63 100644
--- a/servers/physics_3d/area_pair_3d_sw.h
+++ b/servers/physics_3d/area_pair_3d_sw.h
@@ -34,6 +34,7 @@
#include "area_3d_sw.h"
#include "body_3d_sw.h"
#include "constraint_3d_sw.h"
+#include "soft_body_3d_sw.h"
class AreaPair3DSW : public Constraint3DSW {
Body3DSW *body;
@@ -69,4 +70,21 @@ public:
~Area2Pair3DSW();
};
+class AreaSoftBodyPair3DSW : public Constraint3DSW {
+ SoftBody3DSW *soft_body;
+ Area3DSW *area;
+ int soft_body_shape;
+ int area_shape;
+ bool colliding = false;
+ bool process_collision = false;
+
+public:
+ virtual bool setup(real_t p_step) override;
+ virtual bool pre_solve(real_t p_step) override;
+ virtual void solve(real_t p_step) override;
+
+ AreaSoftBodyPair3DSW(SoftBody3DSW *p_sof_body, int p_soft_body_shape, Area3DSW *p_area, int p_area_shape);
+ ~AreaSoftBodyPair3DSW();
+};
+
#endif // AREA_PAIR__SW_H
diff --git a/servers/physics_3d/body_3d_sw.h b/servers/physics_3d/body_3d_sw.h
index bdc615ab6c..efb114a325 100644
--- a/servers/physics_3d/body_3d_sw.h
+++ b/servers/physics_3d/body_3d_sw.h
@@ -96,18 +96,6 @@ class Body3DSW : public CollisionObject3DSW {
Map<Constraint3DSW *, int> constraint_map;
- struct AreaCMP {
- Area3DSW *area;
- int refCount;
- _FORCE_INLINE_ bool operator==(const AreaCMP &p_cmp) const { return area->get_self() == p_cmp.area->get_self(); }
- _FORCE_INLINE_ bool operator<(const AreaCMP &p_cmp) const { return area->get_priority() < p_cmp.area->get_priority(); }
- _FORCE_INLINE_ AreaCMP() {}
- _FORCE_INLINE_ AreaCMP(Area3DSW *p_area) {
- area = p_area;
- refCount = 1;
- }
- };
-
Vector<AreaCMP> areas;
struct Contact {
diff --git a/servers/physics_3d/collision_solver_3d_sw.cpp b/servers/physics_3d/collision_solver_3d_sw.cpp
index 161e7e101b..dcecac1c73 100644
--- a/servers/physics_3d/collision_solver_3d_sw.cpp
+++ b/servers/physics_3d/collision_solver_3d_sw.cpp
@@ -102,6 +102,10 @@ void CollisionSolver3DSW::soft_body_contact_callback(const Vector3 &p_point_A, i
++cinfo.contact_count;
+ if (!cinfo.result_callback) {
+ return;
+ }
+
if (cinfo.swap_result) {
cinfo.result_callback(p_point_B, cinfo.node_index, p_point_A, p_index_A, cinfo.userdata);
} else {
diff --git a/servers/physics_3d/soft_body_3d_sw.cpp b/servers/physics_3d/soft_body_3d_sw.cpp
index 724125bea8..73b81444e1 100644
--- a/servers/physics_3d/soft_body_3d_sw.cpp
+++ b/servers/physics_3d/soft_body_3d_sw.cpp
@@ -928,6 +928,19 @@ void SoftBody3DSW::apply_forces() {
}
}
+void SoftBody3DSW::_compute_area_gravity(const Area3DSW *p_area) {
+ if (p_area->is_gravity_point()) {
+ if (p_area->get_gravity_distance_scale() > 0) {
+ Vector3 v = p_area->get_transform().xform(p_area->get_gravity_vector()) - get_transform().get_origin();
+ gravity += v.normalized() * (p_area->get_gravity() / Math::pow(v.length() * p_area->get_gravity_distance_scale() + 1, 2));
+ } else {
+ gravity += (p_area->get_transform().xform(p_area->get_gravity_vector()) - get_transform().get_origin()).normalized() * p_area->get_gravity();
+ }
+ } else {
+ gravity += p_area->get_gravity_vector() * p_area->get_gravity();
+ }
+}
+
void SoftBody3DSW::predict_motion(real_t p_delta) {
const real_t inv_delta = 1.0 / p_delta;
@@ -935,9 +948,35 @@ void SoftBody3DSW::predict_motion(real_t p_delta) {
Area3DSW *def_area = get_space()->get_default_area();
ERR_FAIL_COND(!def_area);
+ gravity = def_area->get_gravity_vector() * def_area->get_gravity();
+
+ int ac = areas.size();
+ bool stopped = false;
+
+ if (ac) {
+ areas.sort();
+ const AreaCMP *aa = &areas[0];
+ for (int i = ac - 1; i >= 0 && !stopped; i--) {
+ PhysicsServer3D::AreaSpaceOverrideMode mode = aa[i].area->get_space_override_mode();
+ switch (mode) {
+ case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE:
+ case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE: {
+ _compute_area_gravity(aa[i].area);
+ stopped = mode == PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE;
+ } break;
+ case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE:
+ case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE_COMBINE: {
+ gravity = Vector3(0, 0, 0);
+ _compute_area_gravity(aa[i].area);
+ stopped = mode == PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE;
+ } break;
+ default: {
+ }
+ }
+ }
+ }
// Apply forces.
- Vector3 gravity = def_area->get_gravity_vector() * def_area->get_gravity();
add_velocity(gravity * p_delta);
apply_forces();
diff --git a/servers/physics_3d/soft_body_3d_sw.h b/servers/physics_3d/soft_body_3d_sw.h
index ac8bcbf0b9..be0620c506 100644
--- a/servers/physics_3d/soft_body_3d_sw.h
+++ b/servers/physics_3d/soft_body_3d_sw.h
@@ -31,6 +31,7 @@
#ifndef SOFT_BODY_3D_SW_H
#define SOFT_BODY_3D_SW_H
+#include "area_3d_sw.h"
#include "collision_object_3d_sw.h"
#include "core/math/aabb.h"
@@ -100,14 +101,20 @@ class SoftBody3DSW : public CollisionObject3DSW {
real_t drag_coefficient = 0.0; // [0,1]
LocalVector<int> pinned_vertices;
+ Vector3 gravity;
+
SelfList<SoftBody3DSW> active_list;
Set<Constraint3DSW *> constraints;
+ Vector<AreaCMP> areas;
+
VSet<RID> exceptions;
uint64_t island_step = 0;
+ _FORCE_INLINE_ void _compute_area_gravity(const Area3DSW *p_area);
+
public:
SoftBody3DSW();
@@ -129,6 +136,25 @@ public:
_FORCE_INLINE_ uint64_t get_island_step() const { return island_step; }
_FORCE_INLINE_ void set_island_step(uint64_t p_step) { island_step = p_step; }
+ _FORCE_INLINE_ void add_area(Area3DSW *p_area) {
+ int index = areas.find(AreaCMP(p_area));
+ if (index > -1) {
+ areas.write[index].refCount += 1;
+ } else {
+ areas.ordered_insert(AreaCMP(p_area));
+ }
+ }
+
+ _FORCE_INLINE_ void remove_area(Area3DSW *p_area) {
+ int index = areas.find(AreaCMP(p_area));
+ if (index > -1) {
+ areas.write[index].refCount -= 1;
+ if (areas[index].refCount < 1) {
+ areas.remove(index);
+ }
+ }
+ }
+
virtual void set_space(Space3DSW *p_space);
void set_mesh(const Ref<Mesh> &p_mesh);
diff --git a/servers/physics_3d/space_3d_sw.cpp b/servers/physics_3d/space_3d_sw.cpp
index 730460d66a..b90730ad23 100644
--- a/servers/physics_3d/space_3d_sw.cpp
+++ b/servers/physics_3d/space_3d_sw.cpp
@@ -921,7 +921,9 @@ void *Space3DSW::_broadphase_pair(CollisionObject3DSW *A, int p_subindex_A, Coll
Area2Pair3DSW *area2_pair = memnew(Area2Pair3DSW(area_b, p_subindex_B, area, p_subindex_A));
return area2_pair;
} else if (type_B == CollisionObject3DSW::TYPE_SOFT_BODY) {
- // Area/Soft Body, not supported.
+ SoftBody3DSW *softbody = static_cast<SoftBody3DSW *>(B);
+ AreaSoftBodyPair3DSW *soft_area_pair = memnew(AreaSoftBodyPair3DSW(softbody, p_subindex_B, area, p_subindex_A));
+ return soft_area_pair;
} else {
Body3DSW *body = static_cast<Body3DSW *>(B);
AreaPair3DSW *area_pair = memnew(AreaPair3DSW(body, p_subindex_B, area, p_subindex_A));
generated by cgit v1.2.3 (git 2.25.1) at 2024-11-08 21:57:46 +0000