asgardius/virtualx-engine
1
0
Fork 0
Code Issues Pull requests Projects Releases 1 Packages Wiki Activity Actions

Enabled softbody-area collision (intersection) and enabled area-specific gravity for soft bodies.

Browse source
This commit is contained in:
Jeffrey Cochran 2021-07-20 00:23:32 -04:00
parent 1ed00dca88
commit 14640fb6c5
9 changed files with 215 additions and 14 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

8
servers/physics_3d/area_3d_sw.cpp
View file

@ -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();

34
servers/physics_3d/area_3d_sw.h
View file

@ -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

82
servers/physics_3d/area_pair_3d_sw.cpp
View file

@ -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);
}

18
servers/physics_3d/area_pair_3d_sw.h
View file

@ -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

12
servers/physics_3d/body_3d_sw.h
View file

@ -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 {

4
servers/physics_3d/collision_solver_3d_sw.cpp
View file

@ -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 {

41
servers/physics_3d/soft_body_3d_sw.cpp
View file

@ -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();

26
servers/physics_3d/soft_body_3d_sw.h
View file

@ -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);

4
servers/physics_3d/space_3d_sw.cpp
View file

@ -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));