godotengine
/
godot
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge pull request #48221 from nekomatata/solver-multithreaded 

Godot Physics collisions and solver processed on threads
This commit is contained in:
Rémi Verschelde 2021-05-04 09:22:20 +02:00 committed by GitHub
parent 302b6ef576 448c41a3e4
commit 508fbf4abe
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
31 changed files with 894 additions and 466 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

102
servers/physics_2d/area_pair_2d_sw.cpp
View File

@ -40,31 +40,48 @@ bool AreaPair2DSW::setup(real_t p_step) {
result = true; result = true;
} }
process_collision = false;
if (result != colliding) { if (result != colliding) {
if (result) { if (area->get_space_override_mode() != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) {
if (area->get_space_override_mode() != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) { process_collision = true;
body->add_area(area); } else if (area->has_monitor_callback()) {
} process_collision = true;
if (area->has_monitor_callback()) {
area->add_body_to_query(body, body_shape, area_shape);
}
} else {
if (area->get_space_override_mode() != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) {
body->remove_area(area);
}
if (area->has_monitor_callback()) {
area->remove_body_from_query(body, body_shape, area_shape);
}
} }
colliding = result; colliding = result;
} }
return false; //never do any post solving return process_collision;
}
bool AreaPair2DSW::pre_solve(real_t p_step) {
if (!process_collision) {
return false;
}
if (colliding) {
if (area->get_space_override_mode() != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) {
body->add_area(area);
}
if (area->has_monitor_callback()) {
area->add_body_to_query(body, body_shape, area_shape);
}
} else {
if (area->get_space_override_mode() != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) {
body->remove_area(area);
}
if (area->has_monitor_callback()) {
area->remove_body_from_query(body, body_shape, area_shape);
}
}
return false; // Never do any post solving.
} }
void AreaPair2DSW::solve(real_t p_step) { void AreaPair2DSW::solve(real_t p_step) {
// Nothing to do.
} }
AreaPair2DSW::AreaPair2DSW(Body2DSW *p_body, int p_body_shape, Area2DSW *p_area, int p_area_shape) { AreaPair2DSW::AreaPair2DSW(Body2DSW *p_body, int p_body_shape, Area2DSW *p_area, int p_area_shape) {
@ -72,7 +89,6 @@ AreaPair2DSW::AreaPair2DSW(Body2DSW *p_body, int p_body_shape, Area2DSW *p_area,
area = p_area; area = p_area;
body_shape = p_body_shape; body_shape = p_body_shape;
area_shape = p_area_shape; area_shape = p_area_shape;
colliding = false;
body->add_constraint(this, 0); body->add_constraint(this, 0);
area->add_constraint(this); area->add_constraint(this);
if (p_body->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC) { //need to be active to process pair if (p_body->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC) { //need to be active to process pair
@ -103,33 +119,48 @@ bool Area2Pair2DSW::setup(real_t p_step) {
result = true; result = true;
} }
process_collision = false;
if (result != colliding) { if (result != colliding) {
if (result) { if (area_b->has_area_monitor_callback() && area_a->is_monitorable()) {
if (area_b->has_area_monitor_callback() && area_a->is_monitorable()) { process_collision = true;
area_b->add_area_to_query(area_a, shape_a, shape_b); } else if (area_a->has_area_monitor_callback() && area_b->is_monitorable()) {
} process_collision = true;
if (area_a->has_area_monitor_callback() && area_b->is_monitorable()) {
area_a->add_area_to_query(area_b, shape_b, shape_a);
}
} else {
if (area_b->has_area_monitor_callback() && area_a->is_monitorable()) {
area_b->remove_area_from_query(area_a, shape_a, shape_b);
}
if (area_a->has_area_monitor_callback() && area_b->is_monitorable()) {
area_a->remove_area_from_query(area_b, shape_b, shape_a);
}
} }
colliding = result; colliding = result;
} }
return false; //never do any post solving return process_collision;
}
bool Area2Pair2DSW::pre_solve(real_t p_step) {
if (!process_collision) {
return false;
}
if (colliding) {
if (area_b->has_area_monitor_callback() && area_a->is_monitorable()) {
area_b->add_area_to_query(area_a, shape_a, shape_b);
}
if (area_a->has_area_monitor_callback() && area_b->is_monitorable()) {
area_a->add_area_to_query(area_b, shape_b, shape_a);
}
} else {
if (area_b->has_area_monitor_callback() && area_a->is_monitorable()) {
area_b->remove_area_from_query(area_a, shape_a, shape_b);
}
if (area_a->has_area_monitor_callback() && area_b->is_monitorable()) {
area_a->remove_area_from_query(area_b, shape_b, shape_a);
}
}
return false; // Never do any post solving.
} }
void Area2Pair2DSW::solve(real_t p_step) { void Area2Pair2DSW::solve(real_t p_step) {
// Nothing to do.
} }
Area2Pair2DSW::Area2Pair2DSW(Area2DSW *p_area_a, int p_shape_a, Area2DSW *p_area_b, int p_shape_b) { Area2Pair2DSW::Area2Pair2DSW(Area2DSW *p_area_a, int p_shape_a, Area2DSW *p_area_b, int p_shape_b) {
@ -137,7 +168,6 @@ Area2Pair2DSW::Area2Pair2DSW(Area2DSW *p_area_a, int p_shape_a, Area2DSW *p_area
area_b = p_area_b; area_b = p_area_b;
shape_a = p_shape_a; shape_a = p_shape_a;
shape_b = p_shape_b; shape_b = p_shape_b;
colliding = false;
area_a->add_constraint(this); area_a->add_constraint(this);
area_b->add_constraint(this); area_b->add_constraint(this);
} }

32
servers/physics_2d/area_pair_2d_sw.h
View File

@ -36,30 +36,34 @@
#include "constraint_2d_sw.h" #include "constraint_2d_sw.h"
class AreaPair2DSW : public Constraint2DSW { class AreaPair2DSW : public Constraint2DSW {
Body2DSW *body; Body2DSW *body = nullptr;
Area2DSW *area; Area2DSW *area = nullptr;
int body_shape; int body_shape = 0;
int area_shape; int area_shape = 0;
bool colliding; bool colliding = false;
bool process_collision = false;
public: public:
bool setup(real_t p_step); virtual bool setup(real_t p_step) override;
void solve(real_t p_step); virtual bool pre_solve(real_t p_step) override;
virtual void solve(real_t p_step) override;
AreaPair2DSW(Body2DSW *p_body, int p_body_shape, Area2DSW *p_area, int p_area_shape); AreaPair2DSW(Body2DSW *p_body, int p_body_shape, Area2DSW *p_area, int p_area_shape);
~AreaPair2DSW(); ~AreaPair2DSW();
}; };
class Area2Pair2DSW : public Constraint2DSW { class Area2Pair2DSW : public Constraint2DSW {
Area2DSW *area_a; Area2DSW *area_a = nullptr;
Area2DSW *area_b; Area2DSW *area_b = nullptr;
int shape_a; int shape_a = 0;
int shape_b; int shape_b = 0;
bool colliding; bool colliding = false;
bool process_collision = false;
public: public:
bool setup(real_t p_step); virtual bool setup(real_t p_step) override;
void solve(real_t p_step); virtual bool pre_solve(real_t p_step) override;
virtual void solve(real_t p_step) override;
Area2Pair2DSW(Area2DSW *p_area_a, int p_shape_a, Area2DSW *p_area_b, int p_shape_b); Area2Pair2DSW(Area2DSW *p_area_a, int p_shape_a, Area2DSW *p_area_b, int p_shape_b);
~Area2Pair2DSW(); ~Area2Pair2DSW();

35
servers/physics_2d/body_2d_sw.cpp
View File

@ -104,31 +104,17 @@ void Body2DSW::set_active(bool p_active) {
} }
active = p_active; active = p_active;
if (!p_active) {
if (get_space()) { if (active) {
get_space()->body_remove_from_active_list(&active_list);
}
} else {
if (mode == PhysicsServer2D::BODY_MODE_STATIC) { if (mode == PhysicsServer2D::BODY_MODE_STATIC) {
return; //static bodies can't become active // Static bodies can't be active.
} active = false;
if (get_space()) { } else if (get_space()) {
get_space()->body_add_to_active_list(&active_list); get_space()->body_add_to_active_list(&active_list);
} }
} else if (get_space()) {
//still_time=0; get_space()->body_remove_from_active_list(&active_list);
} }
/*
if (!space)
return;
for(int i=0;i<get_shape_count();i++) {
Shape &s=shapes[i];
if (s.bpid>0) {
get_space()->get_broadphase()->set_active(s.bpid,active);
}
}
*/
} }
void Body2DSW::set_param(PhysicsServer2D::BodyParameter p_param, real_t p_value) { void Body2DSW::set_param(PhysicsServer2D::BodyParameter p_param, real_t p_value) {
@ -370,13 +356,6 @@ void Body2DSW::set_space(Space2DSW *p_space) {
if (active) { if (active) {
get_space()->body_add_to_active_list(&active_list); get_space()->body_add_to_active_list(&active_list);
} }
/*
_update_queries();
if (is_active()) {
active=false;
set_active(true);
}
*/
} }
first_integration = false; first_integration = false;

89
servers/physics_2d/body_pair_2d_sw.cpp
View File

@ -87,10 +87,13 @@ void BodyPair2DSW::_contact_added_callback(const Vector2 &p_point_A, const Vecto
int least_deep = -1; int least_deep = -1;
real_t min_depth = 1e10; real_t min_depth = 1e10;
const Transform2D &transform_A = A->get_transform();
const Transform2D &transform_B = B->get_transform();
for (int i = 0; i <= contact_count; i++) { for (int i = 0; i <= contact_count; i++) {
Contact &c = (i == contact_count) ? contact : contacts[i]; Contact &c = (i == contact_count) ? contact : contacts[i];
Vector2 global_A = A->get_transform().basis_xform(c.local_A); Vector2 global_A = transform_A.basis_xform(c.local_A);
Vector2 global_B = B->get_transform().basis_xform(c.local_B) + offset_B; Vector2 global_B = transform_B.basis_xform(c.local_B) + offset_B;
Vector2 axis = global_A - global_B; Vector2 axis = global_A - global_B;
real_t depth = axis.dot(c.normal); real_t depth = axis.dot(c.normal);
@ -124,6 +127,9 @@ void BodyPair2DSW::_validate_contacts() {
real_t max_separation = space->get_contact_max_separation(); real_t max_separation = space->get_contact_max_separation();
real_t max_separation2 = max_separation * max_separation; real_t max_separation2 = max_separation * max_separation;
const Transform2D &transform_A = A->get_transform();
const Transform2D &transform_B = B->get_transform();
for (int i = 0; i < contact_count; i++) { for (int i = 0; i < contact_count; i++) {
Contact &c = contacts[i]; Contact &c = contacts[i];
@ -134,8 +140,8 @@ void BodyPair2DSW::_validate_contacts() {
} else { } else {
c.reused = false; c.reused = false;
Vector2 global_A = A->get_transform().basis_xform(c.local_A); Vector2 global_A = transform_A.basis_xform(c.local_A);
Vector2 global_B = B->get_transform().basis_xform(c.local_B) + offset_B; Vector2 global_B = transform_B.basis_xform(c.local_B) + offset_B;
Vector2 axis = global_A - global_B; Vector2 axis = global_A - global_B;
real_t depth = axis.dot(c.normal); real_t depth = axis.dot(c.normal);
@ -220,14 +226,16 @@ real_t combine_friction(Body2DSW *A, Body2DSW *B) {
} }
bool BodyPair2DSW::setup(real_t p_step) { bool BodyPair2DSW::setup(real_t p_step) {
//cannot collide dynamic_A = (A->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC);
dynamic_B = (B->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC);
if (!A->test_collision_mask(B) || A->has_exception(B->get_self()) || B->has_exception(A->get_self())) { if (!A->test_collision_mask(B) || A->has_exception(B->get_self()) || B->has_exception(A->get_self())) {
collided = false; collided = false;
return false; return false;
} }
bool report_contacts_only = false; report_contacts_only = false;
if ((A->get_mode() <= PhysicsServer2D::BODY_MODE_KINEMATIC) && (B->get_mode() <= PhysicsServer2D::BODY_MODE_KINEMATIC)) { if (!dynamic_A && !dynamic_B) {
if ((A->get_max_contacts_reported() > 0) || (B->get_max_contacts_reported() > 0)) { if ((A->get_max_contacts_reported() > 0) || (B->get_max_contacts_reported() > 0)) {
report_contacts_only = true; report_contacts_only = true;
} else { } else {
@ -246,12 +254,12 @@ bool BodyPair2DSW::setup(real_t p_step) {
_validate_contacts(); _validate_contacts();
Vector2 offset_A = A->get_transform().get_origin(); const Vector2 &offset_A = A->get_transform().get_origin();
Transform2D xform_Au = A->get_transform().untranslated(); Transform2D xform_Au = A->get_transform().untranslated();
Transform2D xform_A = xform_Au * A->get_shape_transform(shape_A); Transform2D xform_A = xform_Au * A->get_shape_transform(shape_A);
Transform2D xform_Bu = B->get_transform(); Transform2D xform_Bu = B->get_transform();
xform_Bu.elements[2] -= A->get_transform().get_origin(); xform_Bu.elements[2] -= offset_A;
Transform2D xform_B = xform_Bu * B->get_shape_transform(shape_B); Transform2D xform_B = xform_Bu * B->get_shape_transform(shape_B);
Shape2DSW *shape_A_ptr = A->get_shape(shape_A); Shape2DSW *shape_A_ptr = A->get_shape(shape_A);
@ -272,13 +280,13 @@ bool BodyPair2DSW::setup(real_t p_step) {
if (!collided) { if (!collided) {
//test ccd (currently just a raycast) //test ccd (currently just a raycast)
if (A->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_RAY && A->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC) { if (A->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_RAY && dynamic_A) {
if (_test_ccd(p_step, A, shape_A, xform_A, B, shape_B, xform_B)) { if (_test_ccd(p_step, A, shape_A, xform_A, B, shape_B, xform_B)) {
collided = true; collided = true;
} }
} }
if (B->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_RAY && B->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC) { if (B->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_RAY && dynamic_B) {
if (_test_ccd(p_step, B, shape_B, xform_B, A, shape_A, xform_A, true)) { if (_test_ccd(p_step, B, shape_B, xform_B, A, shape_A, xform_A, true)) {
collided = true; collided = true;
} }
@ -338,9 +346,21 @@ bool BodyPair2DSW::setup(real_t p_step) {
} }
} }
return true;
}
bool BodyPair2DSW::pre_solve(real_t p_step) {
if (!collided || oneway_disabled) {
return false;
}
real_t max_penetration = space->get_contact_max_allowed_penetration(); real_t max_penetration = space->get_contact_max_allowed_penetration();
real_t bias = 0.3; real_t bias = 0.3;
Shape2DSW *shape_A_ptr = A->get_shape(shape_A);
Shape2DSW *shape_B_ptr = B->get_shape(shape_B);
if (shape_A_ptr->get_custom_bias() || shape_B_ptr->get_custom_bias()) { if (shape_A_ptr->get_custom_bias() || shape_B_ptr->get_custom_bias()) {
if (shape_A_ptr->get_custom_bias() == 0) { if (shape_A_ptr->get_custom_bias() == 0) {
bias = shape_B_ptr->get_custom_bias(); bias = shape_B_ptr->get_custom_bias();
@ -351,21 +371,23 @@ bool BodyPair2DSW::setup(real_t p_step) {
} }
} }
cc = 0;
real_t inv_dt = 1.0 / p_step; real_t inv_dt = 1.0 / p_step;
bool do_process = false; bool do_process = false;
const Vector2 &offset_A = A->get_transform().get_origin();
const Transform2D &transform_A = A->get_transform();
const Transform2D &transform_B = B->get_transform();
for (int i = 0; i < contact_count; i++) { for (int i = 0; i < contact_count; i++) {
Contact &c = contacts[i]; Contact &c = contacts[i];
c.active = false; c.active = false;
Vector2 global_A = xform_Au.xform(c.local_A); Vector2 global_A = transform_A.basis_xform(c.local_A);
Vector2 global_B = xform_Bu.xform(c.local_B); Vector2 global_B = transform_B.basis_xform(c.local_B) + offset_B;
real_t depth = c.normal.dot(global_A - global_B); Vector2 axis = global_A - global_B;
real_t depth = axis.dot(c.normal);
if (depth <= 0 || !c.reused) { if (depth <= 0 || !c.reused) {
continue; continue;
@ -396,8 +418,6 @@ bool BodyPair2DSW::setup(real_t p_step) {
continue; continue;
} }
c.active = true;
// Precompute normal mass, tangent mass, and bias. // Precompute normal mass, tangent mass, and bias.
real_t rnA = c.rA.dot(c.normal); real_t rnA = c.rA.dot(c.normal);
real_t rnB = c.rB.dot(c.normal); real_t rnB = c.rB.dot(c.normal);
@ -421,8 +441,12 @@ bool BodyPair2DSW::setup(real_t p_step) {
// Apply normal + friction impulse // Apply normal + friction impulse
Vector2 P = c.acc_normal_impulse * c.normal + c.acc_tangent_impulse * tangent; Vector2 P = c.acc_normal_impulse * c.normal + c.acc_tangent_impulse * tangent;
A->apply_impulse(-P, c.rA); if (dynamic_A) {
B->apply_impulse(P, c.rB); A->apply_impulse(-P, c.rA);
}
if (dynamic_B) {
B->apply_impulse(P, c.rB);
}
} }
#endif #endif
@ -434,6 +458,7 @@ bool BodyPair2DSW::setup(real_t p_step) {
c.bounce = c.bounce * dv.dot(c.normal); c.bounce = c.bounce * dv.dot(c.normal);
} }
c.active = true;
do_process = true; do_process = true;
} }
@ -441,13 +466,12 @@ bool BodyPair2DSW::setup(real_t p_step) {
} }
void BodyPair2DSW::solve(real_t p_step) { void BodyPair2DSW::solve(real_t p_step) {
if (!collided) { if (!collided || oneway_disabled) {
return; return;
} }
for (int i = 0; i < contact_count; ++i) { for (int i = 0; i < contact_count; ++i) {
Contact &c = contacts[i]; Contact &c = contacts[i];
cc++;
if (!c.active) { if (!c.active) {
continue; continue;
@ -474,8 +498,12 @@ void BodyPair2DSW::solve(real_t p_step) {
Vector2 jb = c.normal * (c.acc_bias_impulse - jbnOld); Vector2 jb = c.normal * (c.acc_bias_impulse - jbnOld);
A->apply_bias_impulse(-jb, c.rA); if (dynamic_A) {
B->apply_bias_impulse(jb, c.rB); A->apply_bias_impulse(-jb, c.rA);
}
if (dynamic_B) {
B->apply_bias_impulse(jb, c.rB);
}
real_t jn = -(c.bounce + vn) * c.mass_normal; real_t jn = -(c.bounce + vn) * c.mass_normal;
real_t jnOld = c.acc_normal_impulse; real_t jnOld = c.acc_normal_impulse;
@ -490,8 +518,12 @@ void BodyPair2DSW::solve(real_t p_step) {
Vector2 j = c.normal * (c.acc_normal_impulse - jnOld) + tangent * (c.acc_tangent_impulse - jtOld); Vector2 j = c.normal * (c.acc_normal_impulse - jnOld) + tangent * (c.acc_tangent_impulse - jtOld);
A->apply_impulse(-j, c.rA); if (dynamic_A) {
B->apply_impulse(j, c.rB); A->apply_impulse(-j, c.rA);
}
if (dynamic_B) {
B->apply_impulse(j, c.rB);
}
} }
} }
@ -504,9 +536,6 @@ BodyPair2DSW::BodyPair2DSW(Body2DSW *p_A, int p_shape_A, Body2DSW *p_B, int p_sh
space = A->get_space(); space = A->get_space();
A->add_constraint(this, 0); A->add_constraint(this, 0);
B->add_constraint(this, 1); B->add_constraint(this, 1);
contact_count = 0;
collided = false;
oneway_disabled = false;
} }
BodyPair2DSW::~BodyPair2DSW() { BodyPair2DSW::~BodyPair2DSW() {

24
servers/physics_2d/body_pair_2d_sw.h
View File

@ -44,13 +44,16 @@ class BodyPair2DSW : public Constraint2DSW {
Body2DSW *B; Body2DSW *B;
}; };
Body2DSW *_arr[2]; Body2DSW *_arr[2] = { nullptr, nullptr };
}; };
int shape_A; int shape_A = 0;
int shape_B; int shape_B = 0;
Space2DSW *space; bool dynamic_A = false;
bool dynamic_B = false;
Space2DSW *space = nullptr;
struct Contact { struct Contact {
Vector2 position; Vector2 position;
@ -73,10 +76,10 @@ class BodyPair2DSW : public Constraint2DSW {
Vector2 sep_axis; Vector2 sep_axis;
Contact contacts[MAX_CONTACTS]; Contact contacts[MAX_CONTACTS];
int contact_count; int contact_count = 0;
bool collided; bool collided = false;
bool oneway_disabled; bool oneway_disabled = false;
int cc; bool report_contacts_only = false;
bool _test_ccd(real_t p_step, Body2DSW *p_A, int p_shape_A, const Transform2D &p_xform_A, Body2DSW *p_B, int p_shape_B, const Transform2D &p_xform_B, bool p_swap_result = false); bool _test_ccd(real_t p_step, Body2DSW *p_A, int p_shape_A, const Transform2D &p_xform_A, Body2DSW *p_B, int p_shape_B, const Transform2D &p_xform_B, bool p_swap_result = false);
void _validate_contacts(); void _validate_contacts();
@ -84,8 +87,9 @@ class BodyPair2DSW : public Constraint2DSW {
_FORCE_INLINE_ void _contact_added_callback(const Vector2 &p_point_A, const Vector2 &p_point_B); _FORCE_INLINE_ void _contact_added_callback(const Vector2 &p_point_A, const Vector2 &p_point_B);
public: public:
bool setup(real_t p_step); virtual bool setup(real_t p_step) override;
void solve(real_t p_step); virtual bool pre_solve(real_t p_step) override;
virtual void solve(real_t p_step) override;
BodyPair2DSW(Body2DSW *p_A, int p_shape_A, Body2DSW *p_B, int p_shape_B); BodyPair2DSW(Body2DSW *p_A, int p_shape_A, Body2DSW *p_B, int p_shape_B);
~BodyPair2DSW(); ~BodyPair2DSW();

4
servers/physics_2d/collision_object_2d_sw.h
View File

@ -143,8 +143,8 @@ public:
return shapes[p_index].metadata; return shapes[p_index].metadata;
} }
_FORCE_INLINE_ Transform2D get_transform() const { return transform; } _FORCE_INLINE_ const Transform2D &get_transform() const { return transform; }
_FORCE_INLINE_ Transform2D get_inv_transform() const { return inv_transform; } _FORCE_INLINE_ const Transform2D &get_inv_transform() const { return inv_transform; }
_FORCE_INLINE_ Space2DSW *get_space() const { return space; } _FORCE_INLINE_ Space2DSW *get_space() const { return space; }
void set_shape_as_disabled(int p_idx, bool p_disabled); void set_shape_as_disabled(int p_idx, bool p_disabled);

1
servers/physics_2d/constraint_2d_sw.h
View File

@ -63,6 +63,7 @@ public:
_FORCE_INLINE_ bool is_disabled_collisions_between_bodies() const { return disabled_collisions_between_bodies; } _FORCE_INLINE_ bool is_disabled_collisions_between_bodies() const { return disabled_collisions_between_bodies; }
virtual bool setup(real_t p_step) = 0; virtual bool setup(real_t p_step) = 0;
virtual bool pre_solve(real_t p_step) = 0;
virtual void solve(real_t p_step) = 0; virtual void solve(real_t p_step) = 0;
virtual ~Constraint2DSW() {} virtual ~Constraint2DSW() {}

92
servers/physics_2d/joints_2d_sw.cpp
View File

@ -97,7 +97,10 @@ normal_relative_velocity(Body2DSW *a, Body2DSW *b, Vector2 rA, Vector2 rB, Vecto
} }
bool PinJoint2DSW::setup(real_t p_step) { bool PinJoint2DSW::setup(real_t p_step) {
if ((A->get_mode() <= PhysicsServer2D::BODY_MODE_KINEMATIC) && (B->get_mode() <= PhysicsServer2D::BODY_MODE_KINEMATIC)) { dynamic_A = (A->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC);
dynamic_B = (B->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC);
if (!dynamic_A && !dynamic_B) {
return false; return false;
} }
@ -148,12 +151,6 @@ bool PinJoint2DSW::setup(real_t p_step) {
bias = delta * -(get_bias() == 0 ? space->get_constraint_bias() : get_bias()) * (1.0 / p_step); bias = delta * -(get_bias() == 0 ? space->get_constraint_bias() : get_bias()) * (1.0 / p_step);
// apply accumulated impulse
A->apply_impulse(-P, rA);
if (B) {
B->apply_impulse(P, rB);
}
return true; return true;
} }
@ -161,6 +158,18 @@ inline Vector2 custom_cross(const Vector2 &p_vec, real_t p_other) {
return Vector2(p_other * p_vec.y, -p_other * p_vec.x); return Vector2(p_other * p_vec.y, -p_other * p_vec.x);
} }
bool PinJoint2DSW::pre_solve(real_t p_step) {
// Apply accumulated impulse.
if (dynamic_A) {
A->apply_impulse(-P, rA);
}
if (B && dynamic_B) {
B->apply_impulse(P, rB);
}
return true;
}
void PinJoint2DSW::solve(real_t p_step) { void PinJoint2DSW::solve(real_t p_step) {
// compute relative velocity // compute relative velocity
Vector2 vA = A->get_linear_velocity() - custom_cross(rA, A->get_angular_velocity()); Vector2 vA = A->get_linear_velocity() - custom_cross(rA, A->get_angular_velocity());
@ -174,8 +183,10 @@ void PinJoint2DSW::solve(real_t p_step) {
Vector2 impulse = M.basis_xform(bias - rel_vel - Vector2(softness, softness) * P); Vector2 impulse = M.basis_xform(bias - rel_vel - Vector2(softness, softness) * P);
A->apply_impulse(-impulse, rA); if (dynamic_A) {
if (B) { A->apply_impulse(-impulse, rA);
}
if (B && dynamic_B) {
B->apply_impulse(impulse, rB); B->apply_impulse(impulse, rB);
} }
@ -262,14 +273,19 @@ mult_k(const Vector2 &vr, const Vector2 &k1, const Vector2 &k2) {
} }
bool GrooveJoint2DSW::setup(real_t p_step) { bool GrooveJoint2DSW::setup(real_t p_step) {
if ((A->get_mode() <= PhysicsServer2D::BODY_MODE_KINEMATIC) && (B->get_mode() <= PhysicsServer2D::BODY_MODE_KINEMATIC)) { dynamic_A = (A->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC);
dynamic_B = (B->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC);
if (!dynamic_A && !dynamic_B) {
return false; return false;
} }
Space2DSW *space = A->get_space();
ERR_FAIL_COND_V(!space, false);
// calculate endpoints in worldspace // calculate endpoints in worldspace
Vector2 ta = A->get_transform().xform(A_groove_1); Vector2 ta = A->get_transform().xform(A_groove_1);
Vector2 tb = A->get_transform().xform(A_groove_2); Vector2 tb = A->get_transform().xform(A_groove_2);
Space2DSW *space = A->get_space();
// calculate axis // calculate axis
Vector2 n = -(tb - ta).orthogonal().normalized(); Vector2 n = -(tb - ta).orthogonal().normalized();
@ -308,14 +324,22 @@ bool GrooveJoint2DSW::setup(real_t p_step) {
real_t _b = get_bias(); real_t _b = get_bias();
gbias = (delta * -(_b == 0 ? space->get_constraint_bias() : _b) * (1.0 / p_step)).clamped(get_max_bias()); gbias = (delta * -(_b == 0 ? space->get_constraint_bias() : _b) * (1.0 / p_step)).clamped(get_max_bias());
// apply accumulated impulse
A->apply_impulse(-jn_acc, rA);
B->apply_impulse(jn_acc, rB);
correct = true; correct = true;
return true; return true;
} }
bool GrooveJoint2DSW::pre_solve(real_t p_step) {
// Apply accumulated impulse.
if (dynamic_A) {
A->apply_impulse(-jn_acc, rA);
}
if (dynamic_B) {
B->apply_impulse(jn_acc, rB);
}
return true;
}
void GrooveJoint2DSW::solve(real_t p_step) { void GrooveJoint2DSW::solve(real_t p_step) {
// compute impulse // compute impulse
Vector2 vr = relative_velocity(A, B, rA, rB); Vector2 vr = relative_velocity(A, B, rA, rB);
@ -328,8 +352,12 @@ void GrooveJoint2DSW::solve(real_t p_step) {
j = jn_acc - jOld; j = jn_acc - jOld;
A->apply_impulse(-j, rA); if (dynamic_A) {
B->apply_impulse(j, rB); A->apply_impulse(-j, rA);
}
if (dynamic_B) {
B->apply_impulse(j, rB);
}
} }
GrooveJoint2DSW::GrooveJoint2DSW(const Vector2 &p_a_groove1, const Vector2 &p_a_groove2, const Vector2 &p_b_anchor, Body2DSW *p_body_a, Body2DSW *p_body_b) : GrooveJoint2DSW::GrooveJoint2DSW(const Vector2 &p_a_groove1, const Vector2 &p_a_groove2, const Vector2 &p_b_anchor, Body2DSW *p_body_a, Body2DSW *p_body_b) :
@ -351,7 +379,10 @@ GrooveJoint2DSW::GrooveJoint2DSW(const Vector2 &p_a_groove1, const Vector2 &p_a_
////////////////////////////////////////////// //////////////////////////////////////////////
bool DampedSpringJoint2DSW::setup(real_t p_step) { bool DampedSpringJoint2DSW::setup(real_t p_step) {
if ((A->get_mode() <= PhysicsServer2D::BODY_MODE_KINEMATIC) && (B->get_mode() <= PhysicsServer2D::BODY_MODE_KINEMATIC)) { dynamic_A = (A->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC);
dynamic_B = (B->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC);
if (!dynamic_A && !dynamic_B) {
return false; return false;
} }
@ -373,12 +404,21 @@ bool DampedSpringJoint2DSW::setup(real_t p_step) {
target_vrn = 0.0f; target_vrn = 0.0f;
v_coef = 1.0f - Math::exp(-damping * (p_step)*k); v_coef = 1.0f - Math::exp(-damping * (p_step)*k);
// apply spring force // Calculate spring force.
real_t f_spring = (rest_length - dist) * stiffness; real_t f_spring = (rest_length - dist) * stiffness;
Vector2 j = n * f_spring * (p_step); j = n * f_spring * (p_step);
A->apply_impulse(-j, rA); return true;
B->apply_impulse(j, rB); }
bool DampedSpringJoint2DSW::pre_solve(real_t p_step) {
// Apply spring force.
if (dynamic_A) {
A->apply_impulse(-j, rA);
}
if (dynamic_B) {
B->apply_impulse(j, rB);
}
return true; return true;
} }
@ -393,8 +433,12 @@ void DampedSpringJoint2DSW::solve(real_t p_step) {
target_vrn = vrn + v_damp; target_vrn = vrn + v_damp;
Vector2 j = n * v_damp * n_mass; Vector2 j = n * v_damp * n_mass;
A->apply_impulse(-j, rA); if (dynamic_A) {
B->apply_impulse(j, rB); A->apply_impulse(-j, rA);
}
if (dynamic_B) {
B->apply_impulse(j, rB);
}
} }
void DampedSpringJoint2DSW::set_param(PhysicsServer2D::DampedSpringParam p_param, real_t p_value) { void DampedSpringJoint2DSW::set_param(PhysicsServer2D::DampedSpringParam p_param, real_t p_value) {

31
servers/physics_2d/joints_2d_sw.h
View File

@ -39,6 +39,10 @@ class Joint2DSW : public Constraint2DSW {
real_t bias; real_t bias;
real_t max_bias; real_t max_bias;
protected:
bool dynamic_A = false;
bool dynamic_B = false;
public: public:
_FORCE_INLINE_ void set_max_force(real_t p_force) { max_force = p_force; } _FORCE_INLINE_ void set_max_force(real_t p_force) { max_force = p_force; }
_FORCE_INLINE_ real_t get_max_force() const { return max_force; } _FORCE_INLINE_ real_t get_max_force() const { return max_force; }
@ -49,8 +53,9 @@ public:
_FORCE_INLINE_ void set_max_bias(real_t p_bias) { max_bias = p_bias; } _FORCE_INLINE_ void set_max_bias(real_t p_bias) { max_bias = p_bias; }
_FORCE_INLINE_ real_t get_max_bias() const { return max_bias; } _FORCE_INLINE_ real_t get_max_bias() const { return max_bias; }
virtual bool setup(real_t p_step) { return false; } virtual bool setup(real_t p_step) override { return false; }
virtual void solve(real_t p_step) {} virtual bool pre_solve(real_t p_step) override { return false; }
virtual void solve(real_t p_step) override {}
void copy_settings_from(Joint2DSW *p_joint); void copy_settings_from(Joint2DSW *p_joint);
@ -90,10 +95,11 @@ class PinJoint2DSW : public Joint2DSW {
real_t softness; real_t softness;
public: public:
virtual PhysicsServer2D::JointType get_type() const { return PhysicsServer2D::JOINT_TYPE_PIN; } virtual PhysicsServer2D::JointType get_type() const override { return PhysicsServer2D::JOINT_TYPE_PIN; }
virtual bool setup(real_t p_step); virtual bool setup(real_t p_step) override;
virtual void solve(real_t p_step); virtual bool pre_solve(real_t p_step) override;
virtual void solve(real_t p_step) override;
void set_param(PhysicsServer2D::PinJointParam p_param, real_t p_value); void set_param(PhysicsServer2D::PinJointParam p_param, real_t p_value);
real_t get_param(PhysicsServer2D::PinJointParam p_param) const; real_t get_param(PhysicsServer2D::PinJointParam p_param) const;
@ -126,10 +132,11 @@ class GrooveJoint2DSW : public Joint2DSW {
bool correct; bool correct;
public: public:
virtual PhysicsServer2D::JointType get_type() const { return PhysicsServer2D::JOINT_TYPE_GROOVE; } virtual PhysicsServer2D::JointType get_type() const override { return PhysicsServer2D::JOINT_TYPE_GROOVE; }
virtual bool setup(real_t p_step); virtual bool setup(real_t p_step) override;
virtual void solve(real_t p_step); virtual bool pre_solve(real_t p_step) override;
virtual void solve(real_t p_step) override;
GrooveJoint2DSW(const Vector2 &p_a_groove1, const Vector2 &p_a_groove2, const Vector2 &p_b_anchor, Body2DSW *p_body_a, Body2DSW *p_body_b); GrooveJoint2DSW(const Vector2 &p_a_groove1, const Vector2 &p_a_groove2, const Vector2 &p_b_anchor, Body2DSW *p_body_a, Body2DSW *p_body_b);
}; };
@ -153,15 +160,17 @@ class DampedSpringJoint2DSW : public Joint2DSW {
Vector2 rA, rB; Vector2 rA, rB;
Vector2 n; Vector2 n;
Vector2 j;
real_t n_mass; real_t n_mass;
real_t target_vrn; real_t target_vrn;
real_t v_coef; real_t v_coef;
public: public:
virtual PhysicsServer2D::JointType get_type() const { return PhysicsServer2D::JOINT_TYPE_DAMPED_SPRING; } virtual PhysicsServer2D::JointType get_type() const override { return PhysicsServer2D::JOINT_TYPE_DAMPED_SPRING; }
virtual bool setup(real_t p_step); virtual bool setup(real_t p_step) override;
virtual void solve(real_t p_step); virtual bool pre_solve(real_t p_step) override;
virtual void solve(real_t p_step) override;
void set_param(PhysicsServer2D::DampedSpringParam p_param, real_t p_value); void set_param(PhysicsServer2D::DampedSpringParam p_param, real_t p_value);
real_t get_param(PhysicsServer2D::DampedSpringParam p_param) const; real_t get_param(PhysicsServer2D::DampedSpringParam p_param) const;

157
servers/physics_2d/step_2d_sw.cpp
View File

@ -29,45 +29,59 @@
/*************************************************************************/ /*************************************************************************/
#include "step_2d_sw.h" #include "step_2d_sw.h"
#include "core/os/os.h" #include "core/os/os.h"
#define BODY_ISLAND_COUNT_RESERVE 128 #define BODY_ISLAND_COUNT_RESERVE 128
#define BODY_ISLAND_SIZE_RESERVE 512 #define BODY_ISLAND_SIZE_RESERVE 512
#define ISLAND_COUNT_RESERVE 128 #define ISLAND_COUNT_RESERVE 128
#define ISLAND_SIZE_RESERVE 512 #define ISLAND_SIZE_RESERVE 512
#define CONSTRAINT_COUNT_RESERVE 1024
void Step2DSW::_populate_island(Body2DSW *p_body, LocalVector<Body2DSW *> &p_body_island, LocalVector<Constraint2DSW *> &p_constraint_island) { void Step2DSW::_populate_island(Body2DSW *p_body, LocalVector<Body2DSW *> &p_body_island, LocalVector<Constraint2DSW *> &p_constraint_island) {
p_body->set_island_step(_step); p_body->set_island_step(_step);
p_body_island.push_back(p_body);
// Faster with reversed iterations. if (p_body->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC) {
for (const List<Pair<Constraint2DSW *, int>>::Element *E = p_body->get_constraint_list().back(); E; E = E->prev()) { // Only dynamic bodies are tested for activation.
Constraint2DSW *c = (Constraint2DSW *)E->get().first; p_body_island.push_back(p_body);
if (c->get_island_step() == _step) { }
continue; //already processed
for (const List<Pair<Constraint2DSW *, int>>::Element *E = p_body->get_constraint_list().front(); E; E = E->next()) {
Constraint2DSW *constraint = (Constraint2DSW *)E->get().first;
if (constraint->get_island_step() == _step) {
continue; // Already processed.
} }
c->set_island_step(_step); constraint->set_island_step(_step);
p_constraint_island.push_back(c); p_constraint_island.push_back(constraint);
all_constraints.push_back(constraint);
for (int i = 0; i < c->get_body_count(); i++) { for (int i = 0; i < constraint->get_body_count(); i++) {
if (i == E->get().second) { if (i == E->get().second) {
continue; continue;
} }
Body2DSW *b = c->get_body_ptr()[i]; Body2DSW *other_body = constraint->get_body_ptr()[i];
if (b->get_island_step() == _step || b->get_mode() == PhysicsServer2D::BODY_MODE_STATIC || b->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC) { if (other_body->get_island_step() == _step) {
continue; //no go continue; // Already processed.
} }
_populate_island(c->get_body_ptr()[i], p_body_island, p_constraint_island); if (other_body->get_mode() == PhysicsServer2D::BODY_MODE_STATIC) {
continue; // Static bodies don't connect islands.
}
_populate_island(other_body, p_body_island, p_constraint_island);
} }
} }
} }
void Step2DSW::_setup_island(LocalVector<Constraint2DSW *> &p_constraint_island, real_t p_delta) { void Step2DSW::_setup_contraint(uint32_t p_constraint_index, void *p_userdata) {
Constraint2DSW *constraint = all_constraints[p_constraint_index];
constraint->setup(delta);
}
void Step2DSW::_pre_solve_island(LocalVector<Constraint2DSW *> &p_constraint_island) const {
uint32_t constraint_count = p_constraint_island.size(); uint32_t constraint_count = p_constraint_island.size();
uint32_t valid_constraint_count = 0; uint32_t valid_constraint_count = 0;
for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) { for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) {
Constraint2DSW *constraint = p_constraint_island[constraint_index]; Constraint2DSW *constraint = p_constraint_island[constraint_index];
if (p_constraint_island[constraint_index]->setup(p_delta)) { if (p_constraint_island[constraint_index]->pre_solve(delta)) {
// Keep this constraint for solving. // Keep this constraint for solving.
p_constraint_island[valid_constraint_count++] = constraint; p_constraint_island[valid_constraint_count++] = constraint;
} }
@ -75,27 +89,25 @@ void Step2DSW::_setup_island(LocalVector<Constraint2DSW *> &p_constraint_island,
p_constraint_island.resize(valid_constraint_count); p_constraint_island.resize(valid_constraint_count);
} }
void Step2DSW::_solve_island(LocalVector<Constraint2DSW *> &p_constraint_island, int p_iterations, real_t p_delta) { void Step2DSW::_solve_island(uint32_t p_island_index, void *p_userdata) const {
for (int i = 0; i < p_iterations; i++) { const LocalVector<Constraint2DSW *> &constraint_island = constraint_islands[p_island_index];
uint32_t constraint_count = p_constraint_island.size();
for (int i = 0; i < iterations; i++) {
uint32_t constraint_count = constraint_island.size();
for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) { for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) {
p_constraint_island[constraint_index]->solve(p_delta); constraint_island[constraint_index]->solve(delta);
} }
} }
} }
void Step2DSW::_check_suspend(const LocalVector<Body2DSW *> &p_body_island, real_t p_delta) { void Step2DSW::_check_suspend(LocalVector<Body2DSW *> &p_body_island) const {
bool can_sleep = true; bool can_sleep = true;
uint32_t body_count = p_body_island.size(); uint32_t body_count = p_body_island.size();
for (uint32_t body_index = 0; body_index < body_count; ++body_index) { for (uint32_t body_index = 0; body_index < body_count; ++body_index) {
Body2DSW *body = p_body_island[body_index]; Body2DSW *body = p_body_island[body_index];
if (body->get_mode() == PhysicsServer2D::BODY_MODE_STATIC || body->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC) { if (!body->sleep_test(delta)) {
continue; // Ignore for static.
}
if (!body->sleep_test(p_delta)) {
can_sleep = false; can_sleep = false;
} }
} }
@ -104,10 +116,6 @@ void Step2DSW::_check_suspend(const LocalVector<Body2DSW *> &p_body_island, real
for (uint32_t body_index = 0; body_index < body_count; ++body_index) { for (uint32_t body_index = 0; body_index < body_count; ++body_index) {
Body2DSW *body = p_body_island[body_index]; Body2DSW *body = p_body_island[body_index];
if (body->get_mode() == PhysicsServer2D::BODY_MODE_STATIC || body->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC) {
continue; // Ignore for static.
}
bool active = body->is_active(); bool active = body->is_active();
if (active == can_sleep) { if (active == can_sleep) {
@ -121,6 +129,9 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) {
p_space->setup(); //update inertias, etc p_space->setup(); //update inertias, etc
iterations = p_iterations;
delta = p_delta;
const SelfList<Body2DSW>::List *body_list = &p_space->get_active_body_list(); const SelfList<Body2DSW>::List *body_list = &p_space->get_active_body_list();
/* INTEGRATE FORCES */ /* INTEGRATE FORCES */
@ -145,12 +156,39 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) {
profile_begtime = profile_endtime; profile_begtime = profile_endtime;
} }
/* GENERATE CONSTRAINT ISLANDS */ /* GENERATE CONSTRAINT ISLANDS FOR MOVING AREAS */
uint32_t island_count = 0;
const SelfList<Area2DSW>::List &aml = p_space->get_moved_area_list();
while (aml.first()) {
for (const Set<Constraint2DSW *>::Element *E = aml.first()->self()->get_constraints().front(); E; E = E->next()) {
Constraint2DSW *constraint = E->get();
if (constraint->get_island_step() == _step) {
continue;
}
constraint->set_island_step(_step);
// Each constraint can be on a separate island for areas as there's no solving phase.
++island_count;
if (constraint_islands.size() < island_count) {
constraint_islands.resize(island_count);
}
LocalVector<Constraint2DSW *> &constraint_island = constraint_islands[island_count - 1];
constraint_island.clear();
all_constraints.push_back(constraint);
constraint_island.push_back(constraint);
}
p_space->area_remove_from_moved_list((SelfList<Area2DSW> *)aml.first()); //faster to remove here
}
/* GENERATE CONSTRAINT ISLANDS FOR ACTIVE RIGID BODIES */
b = body_list->first(); b = body_list->first();
uint32_t body_island_count = 0; uint32_t body_island_count = 0;
uint32_t island_count = 0;
while (b) { while (b) {
Body2DSW *body = b->self(); Body2DSW *body = b->self();
@ -174,7 +212,9 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) {
_populate_island(body, body_island, constraint_island); _populate_island(body, body_island, constraint_island);
body_islands.push_back(body_island); if (body_island.is_empty()) {
--body_island_count;
}
if (constraint_island.is_empty()) { if (constraint_island.is_empty()) {
--island_count; --island_count;
@ -185,37 +225,16 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) {
p_space->set_island_count((int)island_count); p_space->set_island_count((int)island_count);
const SelfList<Area2DSW>::List &aml = p_space->get_moved_area_list();
while (aml.first()) {
for (const Set<Constraint2DSW *>::Element *E = aml.first()->self()->get_constraints().front(); E; E = E->next()) {
Constraint2DSW *c = E->get();
if (c->get_island_step() == _step) {
continue;
}
c->set_island_step(_step);
++island_count;
if (constraint_islands.size() < island_count) {
constraint_islands.resize(island_count);
}
LocalVector<Constraint2DSW *> &constraint_island = constraint_islands[island_count - 1];
constraint_island.clear();
constraint_island.push_back(c);
}
p_space->area_remove_from_moved_list((SelfList<Area2DSW> *)aml.first()); //faster to remove here
}
{ //profile { //profile
profile_endtime = OS::get_singleton()->get_ticks_usec(); profile_endtime = OS::get_singleton()->get_ticks_usec();
p_space->set_elapsed_time(Space2DSW::ELAPSED_TIME_GENERATE_ISLANDS, profile_endtime - profile_begtime); p_space->set_elapsed_time(Space2DSW::ELAPSED_TIME_GENERATE_ISLANDS, profile_endtime - profile_begtime);
profile_begtime = profile_endtime; profile_begtime = profile_endtime;
} }
/* SETUP CONSTRAINT ISLANDS */ /* SETUP CONSTRAINTS / PROCESS COLLISIONS */
for (uint32_t island_index = 0; island_index < island_count; ++island_index) { uint32_t total_contraint_count = all_constraints.size();
_setup_island(constraint_islands[island_index], p_delta); work_pool.do_work(total_contraint_count, this, &Step2DSW::_setup_contraint, nullptr);
}
{ //profile { //profile
profile_endtime = OS::get_singleton()->get_ticks_usec(); profile_endtime = OS::get_singleton()->get_ticks_usec();
@ -223,10 +242,21 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) {
profile_begtime = profile_endtime; profile_begtime = profile_endtime;
} }
/* PRE-SOLVE CONSTRAINT ISLANDS */
// Warning: This doesn't run on threads, because it involves thread-unsafe processing.
for (uint32_t island_index = 0; island_index < island_count; ++island_index) {
_pre_solve_island(constraint_islands[island_index]);
}
/* SOLVE CONSTRAINT ISLANDS */ /* SOLVE CONSTRAINT ISLANDS */
for (uint32_t island_index = 0; island_index < island_count; ++island_index) { // Warning: _solve_island modifies the constraint islands for optimization purpose,
_solve_island(constraint_islands[island_index], p_iterations, p_delta); // their content is not reliable after these calls and shouldn't be used anymore.
if (island_count > 1) {
work_pool.do_work(island_count, this, &Step2DSW::_solve_island, nullptr);
} else if (island_count > 0) {
_solve_island(0);
} }
{ //profile { //profile
@ -247,7 +277,7 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) {
/* SLEEP / WAKE UP ISLANDS */ /* SLEEP / WAKE UP ISLANDS */
for (uint32_t island_index = 0; island_index < body_island_count; ++island_index) { for (uint32_t island_index = 0; island_index < body_island_count; ++island_index) {
_check_suspend(body_islands[island_index], p_delta); _check_suspend(body_islands[island_index]);
} }
{ //profile { //profile
@ -256,6 +286,8 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) {
//profile_begtime=profile_endtime; //profile_begtime=profile_endtime;
} }
all_constraints.clear();
p_space->update(); p_space->update();
p_space->unlock(); p_space->unlock();
_step++; _step++;
@ -266,4 +298,11 @@ Step2DSW::Step2DSW() {
body_islands.reserve(BODY_ISLAND_COUNT_RESERVE); body_islands.reserve(BODY_ISLAND_COUNT_RESERVE);
constraint_islands.reserve(ISLAND_COUNT_RESERVE); constraint_islands.reserve(ISLAND_COUNT_RESERVE);
all_constraints.reserve(CONSTRAINT_COUNT_RESERVE);
work_pool.init();
}
Step2DSW::~Step2DSW() {
work_pool.finish();
} }

15
servers/physics_2d/step_2d_sw.h
View File

@ -34,21 +34,30 @@
#include "space_2d_sw.h" #include "space_2d_sw.h"
#include "core/templates/local_vector.h" #include "core/templates/local_vector.h"
#include "core/templates/thread_work_pool.h"
class Step2DSW { class Step2DSW {
uint64_t _step; uint64_t _step;
int iterations = 0;
real_t delta = 0.0;
ThreadWorkPool work_pool;
LocalVector<LocalVector<Body2DSW *>> body_islands; LocalVector<LocalVector<Body2DSW *>> body_islands;
LocalVector<LocalVector<Constraint2DSW *>> constraint_islands; LocalVector<LocalVector<Constraint2DSW *>> constraint_islands;
LocalVector<Constraint2DSW *> all_constraints;
void _populate_island(Body2DSW *p_body, LocalVector<Body2DSW *> &p_body_island, LocalVector<Constraint2DSW *> &p_constraint_island); void _populate_island(Body2DSW *p_body, LocalVector<Body2DSW *> &p_body_island, LocalVector<Constraint2DSW *> &p_constraint_island);
void _setup_island(LocalVector<Constraint2DSW *> &p_constraint_island, real_t p_delta); void _setup_contraint(uint32_t p_constraint_index, void *p_userdata = nullptr);
void _solve_island(LocalVector<Constraint2DSW *> &p_constraint_island, int p_iterations, real_t p_delta); void _pre_solve_island(LocalVector<Constraint2DSW *> &p_constraint_island) const;
void _check_suspend(const LocalVector<Body2DSW *> &p_body_island, real_t p_delta); void _solve_island(uint32_t p_island_index, void *p_userdata = nullptr) const;
void _check_suspend(LocalVector<Body2DSW *> &p_body_island) const;
public: public:
void step(Space2DSW *p_space, real_t p_delta, int p_iterations); void step(Space2DSW *p_space, real_t p_delta, int p_iterations);
Step2DSW(); Step2DSW();
~Step2DSW();
}; };
#endif // STEP_2D_SW_H #endif // STEP_2D_SW_H

102
servers/physics_3d/area_pair_3d_sw.cpp
View File

@ -40,31 +40,48 @@ bool AreaPair3DSW::setup(real_t p_step) {
result = true; result = true;
} }
process_collision = false;
if (result != colliding) { if (result != colliding) {
if (result) { if (area->get_space_override_mode() != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) {
if (area->get_space_override_mode() != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) { process_collision = true;
body->add_area(area); } else if (area->has_monitor_callback()) {
} process_collision = true;
if (area->has_monitor_callback()) {
area->add_body_to_query(body, body_shape, area_shape);
}
} else {
if (area->get_space_override_mode() != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) {
body->remove_area(area);
}
if (area->has_monitor_callback()) {
area->remove_body_from_query(body, body_shape, area_shape);
}
} }
colliding = result; colliding = result;
} }
return false; //never do any post solving return process_collision;
}
bool AreaPair3DSW::pre_solve(real_t p_step) {
if (!process_collision) {
return false;
}
if (colliding) {
if (area->get_space_override_mode() != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) {
body->add_area(area);
}
if (area->has_monitor_callback()) {
area->add_body_to_query(body, body_shape, area_shape);
}
} else {
if (area->get_space_override_mode() != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) {
body->remove_area(area);
}
if (area->has_monitor_callback()) {
area->remove_body_from_query(body, body_shape, area_shape);
}
}
return false; // Never do any post solving.
} }
void AreaPair3DSW::solve(real_t p_step) { void AreaPair3DSW::solve(real_t p_step) {
// Nothing to do.
} }
AreaPair3DSW::AreaPair3DSW(Body3DSW *p_body, int p_body_shape, Area3DSW *p_area, int p_area_shape) { AreaPair3DSW::AreaPair3DSW(Body3DSW *p_body, int p_body_shape, Area3DSW *p_area, int p_area_shape) {
@ -72,7 +89,6 @@ AreaPair3DSW::AreaPair3DSW(Body3DSW *p_body, int p_body_shape, Area3DSW *p_area,
area = p_area; area = p_area;
body_shape = p_body_shape; body_shape = p_body_shape;
area_shape = p_area_shape; area_shape = p_area_shape;
colliding = false;
body->add_constraint(this, 0); body->add_constraint(this, 0);
area->add_constraint(this); area->add_constraint(this);
if (p_body->get_mode() == PhysicsServer3D::BODY_MODE_KINEMATIC) { if (p_body->get_mode() == PhysicsServer3D::BODY_MODE_KINEMATIC) {
@ -103,33 +119,48 @@ bool Area2Pair3DSW::setup(real_t p_step) {
result = true; result = true;
} }
process_collision = false;
if (result != colliding) { if (result != colliding) {
if (result) { if (area_b->has_area_monitor_callback() && area_a->is_monitorable()) {
if (area_b->has_area_monitor_callback() && area_a->is_monitorable()) { process_collision = true;
area_b->add_area_to_query(area_a, shape_a, shape_b); } else if (area_a->has_area_monitor_callback() && area_b->is_monitorable()) {
} process_collision = true;
if (area_a->has_area_monitor_callback() && area_b->is_monitorable()) {
area_a->add_area_to_query(area_b, shape_b, shape_a);
}
} else {
if (area_b->has_area_monitor_callback() && area_a->is_monitorable()) {
area_b->remove_area_from_query(area_a, shape_a, shape_b);
}
if (area_a->has_area_monitor_callback() && area_b->is_monitorable()) {
area_a->remove_area_from_query(area_b, shape_b, shape_a);
}
} }
colliding = result; colliding = result;
} }
return false; //never do any post solving return process_collision;
}
bool Area2Pair3DSW::pre_solve(real_t p_step) {
if (!process_collision) {
return false;
}
if (colliding) {
if (area_b->has_area_monitor_callback() && area_a->is_monitorable()) {
area_b->add_area_to_query(area_a, shape_a, shape_b);
}
if (area_a->has_area_monitor_callback() && area_b->is_monitorable()) {
area_a->add_area_to_query(area_b, shape_b, shape_a);
}
} else {
if (area_b->has_area_monitor_callback() && area_a->is_monitorable()) {
area_b->remove_area_from_query(area_a, shape_a, shape_b);
}
if (area_a->has_area_monitor_callback() && area_b->is_monitorable()) {
area_a->remove_area_from_query(area_b, shape_b, shape_a);
}
}
return false; // Never do any post solving.
} }
void Area2Pair3DSW::solve(real_t p_step) { void Area2Pair3DSW::solve(real_t p_step) {
// Nothing to do.
} }
Area2Pair3DSW::Area2Pair3DSW(Area3DSW *p_area_a, int p_shape_a, Area3DSW *p_area_b, int p_shape_b) { Area2Pair3DSW::Area2Pair3DSW(Area3DSW *p_area_a, int p_shape_a, Area3DSW *p_area_b, int p_shape_b) {
@ -137,7 +168,6 @@ Area2Pair3DSW::Area2Pair3DSW(Area3DSW *p_area_a, int p_shape_a, Area3DSW *p_area
area_b = p_area_b; area_b = p_area_b;
shape_a = p_shape_a; shape_a = p_shape_a;
shape_b = p_shape_b; shape_b = p_shape_b;
colliding = false;
area_a->add_constraint(this); area_a->add_constraint(this);
area_b->add_constraint(this); area_b->add_constraint(this);
} }

16
servers/physics_3d/area_pair_3d_sw.h
View File

@ -40,11 +40,13 @@ class AreaPair3DSW : public Constraint3DSW {
Area3DSW *area; Area3DSW *area;
int body_shape; int body_shape;
int area_shape; int area_shape;
bool colliding; bool colliding = false;
bool process_collision = false;
public: public:
bool setup(real_t p_step); virtual bool setup(real_t p_step) override;
void solve(real_t p_step); virtual bool pre_solve(real_t p_step) override;
virtual void solve(real_t p_step) override;
AreaPair3DSW(Body3DSW *p_body, int p_body_shape, Area3DSW *p_area, int p_area_shape); AreaPair3DSW(Body3DSW *p_body, int p_body_shape, Area3DSW *p_area, int p_area_shape);
~AreaPair3DSW(); ~AreaPair3DSW();
@ -55,11 +57,13 @@ class Area2Pair3DSW : public Constraint3DSW {
Area3DSW *area_b; Area3DSW *area_b;
int shape_a; int shape_a;
int shape_b; int shape_b;
bool colliding; bool colliding = false;
bool process_collision = false;
public: public:
bool setup(real_t p_step); virtual bool setup(real_t p_step) override;
void solve(real_t p_step); virtual bool pre_solve(real_t p_step) override;
virtual void solve(real_t p_step) override;
Area2Pair3DSW(Area3DSW *p_area_a, int p_shape_a, Area3DSW *p_area_b, int p_shape_b); Area2Pair3DSW(Area3DSW *p_area_a, int p_shape_a, Area3DSW *p_area_b, int p_shape_b);
~Area2Pair3DSW(); ~Area2Pair3DSW();

35
servers/physics_3d/body_3d_sw.cpp
View File

@ -145,31 +145,17 @@ void Body3DSW::set_active(bool p_active) {
} }
active = p_active; active = p_active;
if (!p_active) {
if (get_space()) { if (active) {
get_space()->body_remove_from_active_list(&active_list);
}
} else {
if (mode == PhysicsServer3D::BODY_MODE_STATIC) { if (mode == PhysicsServer3D::BODY_MODE_STATIC) {
return; //static bodies can't become active // Static bodies can't be active.
} active = false;
if (get_space()) { } else if (get_space()) {
get_space()->body_add_to_active_list(&active_list); get_space()->body_add_to_active_list(&active_list);
} }
} else if (get_space()) {
//still_time=0; get_space()->body_remove_from_active_list(&active_list);
} }
/*
if (!space)
return;
for(int i=0;i<get_shape_count();i++) {
Shape &s=shapes[i];
if (s.bpid>0) {
get_space()->get_broadphase()->set_active(s.bpid,active);
}
}
*/
} }
void Body3DSW::set_param(PhysicsServer3D::BodyParameter p_param, real_t p_value) { void Body3DSW::set_param(PhysicsServer3D::BodyParameter p_param, real_t p_value) {
@ -392,13 +378,6 @@ void Body3DSW::set_space(Space3DSW *p_space) {
if (active) { if (active) {
get_space()->body_add_to_active_list(&active_list); get_space()->body_add_to_active_list(&active_list);
} }
/*
_update_queries();
if (is_active()) {
active=false;
set_active(true);
}
*/
} }
first_integration = true; first_integration = true;

137
servers/physics_3d/body_pair_3d_sw.cpp
View File

@ -212,14 +212,16 @@ real_t combine_friction(Body3DSW *A, Body3DSW *B) {
} }
bool BodyPair3DSW::setup(real_t p_step) { bool BodyPair3DSW::setup(real_t p_step) {
//cannot collide dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
if (!A->test_collision_mask(B) || A->has_exception(B->get_self()) || B->has_exception(A->get_self())) { if (!A->test_collision_mask(B) || A->has_exception(B->get_self()) || B->has_exception(A->get_self())) {
collided = false; collided = false;
return false; return false;
} }
bool report_contacts_only = false; report_contacts_only = false;
if ((A->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC) && (B->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC)) { if (!dynamic_A && !dynamic_B) {
if ((A->get_max_contacts_reported() > 0) || (B->get_max_contacts_reported() > 0)) { if ((A->get_max_contacts_reported() > 0) || (B->get_max_contacts_reported() > 0)) {
report_contacts_only = true; report_contacts_only = true;
} else { } else {
@ -237,7 +239,7 @@ bool BodyPair3DSW::setup(real_t p_step) {
validate_contacts(); validate_contacts();
Vector3 offset_A = A->get_transform().get_origin(); const Vector3 &offset_A = A->get_transform().get_origin();
Transform xform_Au = Transform(A->get_transform().basis, Vector3()); Transform xform_Au = Transform(A->get_transform().basis, Vector3());
Transform xform_A = xform_Au * A->get_shape_transform(shape_A); Transform xform_A = xform_Au * A->get_shape_transform(shape_A);
@ -248,27 +250,37 @@ bool BodyPair3DSW::setup(real_t p_step) {
Shape3DSW *shape_A_ptr = A->get_shape(shape_A); Shape3DSW *shape_A_ptr = A->get_shape(shape_A);
Shape3DSW *shape_B_ptr = B->get_shape(shape_B); Shape3DSW *shape_B_ptr = B->get_shape(shape_B);
bool collided = CollisionSolver3DSW::solve_static(shape_A_ptr, xform_A, shape_B_ptr, xform_B, _contact_added_callback, this, &sep_axis); collided = CollisionSolver3DSW::solve_static(shape_A_ptr, xform_A, shape_B_ptr, xform_B, _contact_added_callback, this, &sep_axis);
this->collided = collided;
if (!collided) { if (!collided) {
//test ccd (currently just a raycast) //test ccd (currently just a raycast)
if (A->is_continuous_collision_detection_enabled() && A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC && B->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC) { if (A->is_continuous_collision_detection_enabled() && dynamic_A && !dynamic_B) {
_test_ccd(p_step, A, shape_A, xform_A, B, shape_B, xform_B); _test_ccd(p_step, A, shape_A, xform_A, B, shape_B, xform_B);
} }
if (B->is_continuous_collision_detection_enabled() && B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC && A->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC) { if (B->is_continuous_collision_detection_enabled() && dynamic_B && !dynamic_A) {
_test_ccd(p_step, B, shape_B, xform_B, A, shape_A, xform_A); _test_ccd(p_step, B, shape_B, xform_B, A, shape_A, xform_A);
} }
return false; return false;
} }
return true;
}
bool BodyPair3DSW::pre_solve(real_t p_step) {
if (!collided) {
return false;
}
real_t max_penetration = space->get_contact_max_allowed_penetration(); real_t max_penetration = space->get_contact_max_allowed_penetration();
real_t bias = (real_t)0.3; real_t bias = (real_t)0.3;
Shape3DSW *shape_A_ptr = A->get_shape(shape_A);
Shape3DSW *shape_B_ptr = B->get_shape(shape_B);
if (shape_A_ptr->get_custom_bias() || shape_B_ptr->get_custom_bias()) { if (shape_A_ptr->get_custom_bias() || shape_B_ptr->get_custom_bias()) {
if (shape_A_ptr->get_custom_bias() == 0) { if (shape_A_ptr->get_custom_bias() == 0) {
bias = shape_B_ptr->get_custom_bias(); bias = shape_B_ptr->get_custom_bias();
@ -283,22 +295,26 @@ bool BodyPair3DSW::setup(real_t p_step) {
bool do_process = false; bool do_process = false;
const Basis &basis_A = A->get_transform().basis;
const Basis &basis_B = B->get_transform().basis;
for (int i = 0; i < contact_count; i++) { for (int i = 0; i < contact_count; i++) {
Contact &c = contacts[i]; Contact &c = contacts[i];
c.active = false; c.active = false;
Vector3 global_A = xform_Au.xform(c.local_A); Vector3 global_A = basis_A.xform(c.local_A);
Vector3 global_B = xform_Bu.xform(c.local_B); Vector3 global_B = basis_B.xform(c.local_B) + offset_B;
real_t depth = c.normal.dot(global_A - global_B); Vector3 axis = global_A - global_B;
real_t depth = axis.dot(c.normal);
if (depth <= 0) { if (depth <= 0) {
continue; continue;
} }
#ifdef DEBUG_ENABLED #ifdef DEBUG_ENABLED
if (space->is_debugging_contacts()) { if (space->is_debugging_contacts()) {
const Vector3 &offset_A = A->get_transform().get_origin();
space->add_debug_contact(global_A + offset_A); space->add_debug_contact(global_A + offset_A);
space->add_debug_contact(global_B + offset_A); space->add_debug_contact(global_B + offset_A);
} }
@ -338,8 +354,12 @@ bool BodyPair3DSW::setup(real_t p_step) {
c.depth = depth; c.depth = depth;
Vector3 j_vec = c.normal * c.acc_normal_impulse + c.acc_tangent_impulse; Vector3 j_vec = c.normal * c.acc_normal_impulse + c.acc_tangent_impulse;
A->apply_impulse(-j_vec, c.rA + A->get_center_of_mass()); if (dynamic_A) {
B->apply_impulse(j_vec, c.rB + B->get_center_of_mass()); A->apply_impulse(-j_vec, c.rA + A->get_center_of_mass());
}
if (dynamic_B) {
B->apply_impulse(j_vec, c.rB + B->get_center_of_mass());
}
c.acc_bias_impulse = 0; c.acc_bias_impulse = 0;
c.acc_bias_impulse_center_of_mass = 0; c.acc_bias_impulse_center_of_mass = 0;
@ -361,6 +381,8 @@ void BodyPair3DSW::solve(real_t p_step) {
return; return;
} }
const real_t max_bias_av = MAX_BIAS_ROTATION / p_step;
for (int i = 0; i < contact_count; i++) { for (int i = 0; i < contact_count; i++) {
Contact &c = contacts[i]; Contact &c = contacts[i];
if (!c.active) { if (!c.active) {
@ -384,8 +406,12 @@ void BodyPair3DSW::solve(real_t p_step) {
Vector3 jb = c.normal * (c.acc_bias_impulse - jbnOld); Vector3 jb = c.normal * (c.acc_bias_impulse - jbnOld);
A->apply_bias_impulse(-jb, c.rA + A->get_center_of_mass(), MAX_BIAS_ROTATION / p_step); if (dynamic_A) {
B->apply_bias_impulse(jb, c.rB + B->get_center_of_mass(), MAX_BIAS_ROTATION / p_step); A->apply_bias_impulse(-jb, c.rA + A->get_center_of_mass(), max_bias_av);
}
if (dynamic_B) {
B->apply_bias_impulse(jb, c.rB + B->get_center_of_mass(), max_bias_av);
}
crbA = A->get_biased_angular_velocity().cross(c.rA); crbA = A->get_biased_angular_velocity().cross(c.rA);
crbB = B->get_biased_angular_velocity().cross(c.rB); crbB = B->get_biased_angular_velocity().cross(c.rB);
@ -400,8 +426,12 @@ void BodyPair3DSW::solve(real_t p_step) {
Vector3 jb_com = c.normal * (c.acc_bias_impulse_center_of_mass - jbnOld_com); Vector3 jb_com = c.normal * (c.acc_bias_impulse_center_of_mass - jbnOld_com);
A->apply_bias_impulse(-jb_com, A->get_center_of_mass(), 0.0f); if (dynamic_A) {
B->apply_bias_impulse(jb_com, B->get_center_of_mass(), 0.0f); A->apply_bias_impulse(-jb_com, A->get_center_of_mass(), 0.0f);
}
if (dynamic_B) {
B->apply_bias_impulse(jb_com, B->get_center_of_mass(), 0.0f);
}
} }
c.active = true; c.active = true;
@ -421,8 +451,12 @@ void BodyPair3DSW::solve(real_t p_step) {
Vector3 j = c.normal * (c.acc_normal_impulse - jnOld); Vector3 j = c.normal * (c.acc_normal_impulse - jnOld);
A->apply_impulse(-j, c.rA + A->get_center_of_mass()); if (dynamic_A) {
B->apply_impulse(j, c.rB + B->get_center_of_mass()); A->apply_impulse(-j, c.rA + A->get_center_of_mass());
}
if (dynamic_B) {
B->apply_impulse(j, c.rB + B->get_center_of_mass());
}
c.active = true; c.active = true;
} }
@ -464,8 +498,12 @@ void BodyPair3DSW::solve(real_t p_step) {
jt = c.acc_tangent_impulse - jtOld; jt = c.acc_tangent_impulse - jtOld;
A->apply_impulse(-jt, c.rA + A->get_center_of_mass()); if (dynamic_A) {
B->apply_impulse(jt, c.rB + B->get_center_of_mass()); A->apply_impulse(-jt, c.rA + A->get_center_of_mass());
}
if (dynamic_B) {
B->apply_impulse(jt, c.rB + B->get_center_of_mass());
}
c.active = true; c.active = true;
} }
@ -481,8 +519,6 @@ BodyPair3DSW::BodyPair3DSW(Body3DSW *p_A, int p_shape_A, Body3DSW *p_B, int p_sh
space = A->get_space(); space = A->get_space();
A->add_constraint(this, 0); A->add_constraint(this, 0);
B->add_constraint(this, 1); B->add_constraint(this, 1);
contact_count = 0;
collided = false;
} }
BodyPair3DSW::~BodyPair3DSW() { BodyPair3DSW::~BodyPair3DSW() {
@ -564,6 +600,8 @@ void BodySoftBodyPair3DSW::validate_contacts() {
} }
bool BodySoftBodyPair3DSW::setup(real_t p_step) { bool BodySoftBodyPair3DSW::setup(real_t p_step) {
body_dynamic = (body->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
if (!body->test_collision_mask(soft_body) || body->has_exception(soft_body->get_self()) || soft_body->has_exception(body->get_self())) { if (!body->test_collision_mask(soft_body) || body->has_exception(soft_body->get_self()) || soft_body->has_exception(body->get_self())) {
collided = false; collided = false;
return false; return false;
@ -585,12 +623,22 @@ bool BodySoftBodyPair3DSW::setup(real_t p_step) {
Shape3DSW *shape_A_ptr = body->get_shape(body_shape); Shape3DSW *shape_A_ptr = body->get_shape(body_shape);
Shape3DSW *shape_B_ptr = soft_body->get_shape(0); Shape3DSW *shape_B_ptr = soft_body->get_shape(0);
bool collided = CollisionSolver3DSW::solve_static(shape_A_ptr, xform_A, shape_B_ptr, xform_B, _contact_added_callback, this, &sep_axis); collided = CollisionSolver3DSW::solve_static(shape_A_ptr, xform_A, shape_B_ptr, xform_B, _contact_added_callback, this, &sep_axis);
this->collided = collided;
return collided;
}
bool BodySoftBodyPair3DSW::pre_solve(real_t p_step) {
if (!collided) {
return false;
}
real_t max_penetration = space->get_contact_max_allowed_penetration(); real_t max_penetration = space->get_contact_max_allowed_penetration();
real_t bias = (real_t)0.3; real_t bias = (real_t)0.3;
Shape3DSW *shape_A_ptr = body->get_shape(body_shape);
if (shape_A_ptr->get_custom_bias()) { if (shape_A_ptr->get_custom_bias()) {
bias = shape_A_ptr->get_custom_bias(); bias = shape_A_ptr->get_custom_bias();
} }
@ -599,6 +647,8 @@ bool BodySoftBodyPair3DSW::setup(real_t p_step) {
bool do_process = false; bool do_process = false;
const Transform &transform_A = body->get_transform();
uint32_t contact_count = contacts.size(); uint32_t contact_count = contacts.size();
for (uint32_t contact_index = 0; contact_index < contact_count; ++contact_index) { for (uint32_t contact_index = 0; contact_index < contact_count; ++contact_index) {
Contact &c = contacts[contact_index]; Contact &c = contacts[contact_index];
@ -609,10 +659,10 @@ bool BodySoftBodyPair3DSW::setup(real_t p_step) {
continue; continue;
} }
Vector3 global_A = xform_Au.xform(c.local_A); Vector3 global_A = transform_A.xform(c.local_A);
Vector3 global_B = soft_body->get_node_position(c.index_B) + c.local_B; Vector3 global_B = soft_body->get_node_position(c.index_B) + c.local_B;
Vector3 axis = global_A - global_B;
real_t depth = c.normal.dot(global_A - global_B); real_t depth = axis.dot(c.normal);
if (depth <= 0) { if (depth <= 0) {
continue; continue;
@ -629,7 +679,7 @@ bool BodySoftBodyPair3DSW::setup(real_t p_step) {
} }
#endif #endif
c.rA = global_A - xform_Au.origin - body->get_center_of_mass(); c.rA = global_A - transform_A.origin - body->get_center_of_mass();
c.rB = global_B; c.rB = global_B;
if (body->can_report_contacts()) { if (body->can_report_contacts()) {
@ -637,7 +687,7 @@ bool BodySoftBodyPair3DSW::setup(real_t p_step) {
body->add_contact(global_A, -c.normal, depth, body_shape, global_B, 0, soft_body->get_instance_id(), soft_body->get_self(), crA); body->add_contact(global_A, -c.normal, depth, body_shape, global_B, 0, soft_body->get_instance_id(), soft_body->get_self(), crA);
} }
if (body->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC) { if (body_dynamic) {
body->set_active(true); body->set_active(true);
} }
@ -651,7 +701,9 @@ bool BodySoftBodyPair3DSW::setup(real_t p_step) {
c.depth = depth; c.depth = depth;
Vector3 j_vec = c.normal * c.acc_normal_impulse + c.acc_tangent_impulse; Vector3 j_vec = c.normal * c.acc_normal_impulse + c.acc_tangent_impulse;
body->apply_impulse(-j_vec, c.rA + body->get_center_of_mass()); if (body_dynamic) {
body->apply_impulse(-j_vec, c.rA + body->get_center_of_mass());
}
soft_body->apply_node_impulse(c.index_B, j_vec); soft_body->apply_node_impulse(c.index_B, j_vec);
c.acc_bias_impulse = 0; c.acc_bias_impulse = 0;
c.acc_bias_impulse_center_of_mass = 0; c.acc_bias_impulse_center_of_mass = 0;
@ -675,6 +727,8 @@ void BodySoftBodyPair3DSW::solve(real_t p_step) {
return; return;
} }
const real_t max_bias_av = MAX_BIAS_ROTATION / p_step;
uint32_t contact_count = contacts.size(); uint32_t contact_count = contacts.size();
for (uint32_t contact_index = 0; contact_index < contact_count; ++contact_index) { for (uint32_t contact_index = 0; contact_index < contact_count; ++contact_index) {
Contact &c = contacts[contact_index]; Contact &c = contacts[contact_index];
@ -697,7 +751,9 @@ void BodySoftBodyPair3DSW::solve(real_t p_step) {
Vector3 jb = c.normal * (c.acc_bias_impulse - jbnOld); Vector3 jb = c.normal * (c.acc_bias_impulse - jbnOld);
body->apply_bias_impulse(-jb, c.rA + body->get_center_of_mass(), MAX_BIAS_ROTATION / p_step); if (body_dynamic) {
body->apply_bias_impulse(-jb, c.rA + body->get_center_of_mass(), max_bias_av);
}
soft_body->apply_node_bias_impulse(c.index_B, jb); soft_body->apply_node_bias_impulse(c.index_B, jb);
crbA = body->get_biased_angular_velocity().cross(c.rA); crbA = body->get_biased_angular_velocity().cross(c.rA);
@ -712,7 +768,9 @@ void BodySoftBodyPair3DSW::solve(real_t p_step) {
Vector3 jb_com = c.normal * (c.acc_bias_impulse_center_of_mass - jbnOld_com); Vector3 jb_com = c.normal * (c.acc_bias_impulse_center_of_mass - jbnOld_com);
body->apply_bias_impulse(-jb_com, body->get_center_of_mass(), 0.0f); if (body_dynamic) {
body->apply_bias_impulse(-jb_com, body->get_center_of_mass(), 0.0f);
}
soft_body->apply_node_bias_impulse(c.index_B, jb_com); soft_body->apply_node_bias_impulse(c.index_B, jb_com);
} }
@ -732,7 +790,9 @@ void BodySoftBodyPair3DSW::solve(real_t p_step) {
Vector3 j = c.normal * (c.acc_normal_impulse - jnOld); Vector3 j = c.normal * (c.acc_normal_impulse - jnOld);
body->apply_impulse(-j, c.rA + body->get_center_of_mass()); if (body_dynamic) {
body->apply_impulse(-j, c.rA + body->get_center_of_mass());
}
soft_body->apply_node_impulse(c.index_B, j); soft_body->apply_node_impulse(c.index_B, j);
c.active = true; c.active = true;
@ -773,7 +833,9 @@ void BodySoftBodyPair3DSW::solve(real_t p_step) {
jt = c.acc_tangent_impulse - jtOld; jt = c.acc_tangent_impulse - jtOld;
body->apply_impulse(-jt, c.rA + body->get_center_of_mass()); if (body_dynamic) {
body->apply_impulse(-jt, c.rA + body->get_center_of_mass());
}
soft_body->apply_node_impulse(c.index_B, jt); soft_body->apply_node_impulse(c.index_B, jt);
c.active = true; c.active = true;
@ -781,7 +843,8 @@ void BodySoftBodyPair3DSW::solve(real_t p_step) {
} }
} }
BodySoftBodyPair3DSW::BodySoftBodyPair3DSW(Body3DSW *p_A, int p_shape_A, SoftBody3DSW *p_B) { BodySoftBodyPair3DSW::BodySoftBodyPair3DSW(Body3DSW *p_A, int p_shape_A, SoftBody3DSW *p_B) :
BodyContact3DSW(&body, 1) {
body = p_A; body = p_A;
soft_body = p_B; soft_body = p_B;
body_shape = p_shape_A; body_shape = p_shape_A;

38
servers/physics_3d/body_pair_3d_sw.h
View File

@ -57,9 +57,9 @@ protected:
}; };
Vector3 sep_axis; Vector3 sep_axis;
bool collided; bool collided = false;
Space3DSW *space; Space3DSW *space = nullptr;
BodyContact3DSW(Body3DSW **p_body_ptr = nullptr, int p_body_count = 0) : BodyContact3DSW(Body3DSW **p_body_ptr = nullptr, int p_body_count = 0) :
Constraint3DSW(p_body_ptr, p_body_count) { Constraint3DSW(p_body_ptr, p_body_count) {
@ -77,16 +77,21 @@ class BodyPair3DSW : public BodyContact3DSW {
Body3DSW *B; Body3DSW *B;
}; };
Body3DSW *_arr[2]; Body3DSW *_arr[2] = { nullptr, nullptr };
}; };
int shape_A; int shape_A = 0;
int shape_B; int shape_B = 0;
bool dynamic_A = false;
bool dynamic_B = false;
bool report_contacts_only = false;
Vector3 offset_B; //use local A coordinates to avoid numerical issues on collision detection Vector3 offset_B; //use local A coordinates to avoid numerical issues on collision detection
Contact contacts[MAX_CONTACTS]; Contact contacts[MAX_CONTACTS];
int contact_count; int contact_count = 0;
static void _contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata); static void _contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata);
@ -96,18 +101,21 @@ class BodyPair3DSW : public BodyContact3DSW {
bool _test_ccd(real_t p_step, Body3DSW *p_A, int p_shape_A, const Transform &p_xform_A, Body3DSW *p_B, int p_shape_B, const Transform &p_xform_B); bool _test_ccd(real_t p_step, Body3DSW *p_A, int p_shape_A, const Transform &p_xform_A, Body3DSW *p_B, int p_shape_B, const Transform &p_xform_B);
public: public:
bool setup(real_t p_step); virtual bool setup(real_t p_step) override;
void solve(real_t p_step); virtual bool pre_solve(real_t p_step) override;
virtual void solve(real_t p_step) override;
BodyPair3DSW(Body3DSW *p_A, int p_shape_A, Body3DSW *p_B, int p_shape_B); BodyPair3DSW(Body3DSW *p_A, int p_shape_A, Body3DSW *p_B, int p_shape_B);
~BodyPair3DSW(); ~BodyPair3DSW();
}; };
class BodySoftBodyPair3DSW : public BodyContact3DSW { class BodySoftBodyPair3DSW : public BodyContact3DSW {
Body3DSW *body; Body3DSW *body = nullptr;
SoftBody3DSW *soft_body; SoftBody3DSW *soft_body = nullptr;
int body_shape; int body_shape = 0;
bool body_dynamic = false;
LocalVector<Contact> contacts; LocalVector<Contact> contacts;
@ -118,8 +126,12 @@ class BodySoftBodyPair3DSW : public BodyContact3DSW {
void validate_contacts(); void validate_contacts();
public: public:
bool setup(real_t p_step); virtual bool setup(real_t p_step) override;
void solve(real_t p_step); virtual bool pre_solve(real_t p_step) override;
virtual void solve(real_t p_step) override;
virtual SoftBody3DSW *get_soft_body_ptr(int p_index) const override { return soft_body; }
virtual int get_soft_body_count() const override { return 1; }
BodySoftBodyPair3DSW(Body3DSW *p_A, int p_shape_A, SoftBody3DSW *p_B); BodySoftBodyPair3DSW(Body3DSW *p_A, int p_shape_A, SoftBody3DSW *p_B);
~BodySoftBodyPair3DSW(); ~BodySoftBodyPair3DSW();

7
servers/physics_3d/constraint_3d_sw.h
View File

@ -31,7 +31,8 @@
#ifndef CONSTRAINT_SW_H #ifndef CONSTRAINT_SW_H
#define CONSTRAINT_SW_H #define CONSTRAINT_SW_H
#include "body_3d_sw.h" class Body3DSW;
class SoftBody3DSW;
class Constraint3DSW { class Constraint3DSW {
Body3DSW **_body_ptr; Body3DSW **_body_ptr;
@ -61,6 +62,9 @@ public:
_FORCE_INLINE_ Body3DSW **get_body_ptr() const { return _body_ptr; } _FORCE_INLINE_ Body3DSW **get_body_ptr() const { return _body_ptr; }
_FORCE_INLINE_ int get_body_count() const { return _body_count; } _FORCE_INLINE_ int get_body_count() const { return _body_count; }
virtual SoftBody3DSW *get_soft_body_ptr(int p_index) const { return nullptr; }
virtual int get_soft_body_count() const { return 0; }
_FORCE_INLINE_ void set_priority(int p_priority) { priority = p_priority; } _FORCE_INLINE_ void set_priority(int p_priority) { priority = p_priority; }
_FORCE_INLINE_ int get_priority() const { return priority; } _FORCE_INLINE_ int get_priority() const { return priority; }
@ -68,6 +72,7 @@ public:
_FORCE_INLINE_ bool is_disabled_collisions_between_bodies() const { return disabled_collisions_between_bodies; } _FORCE_INLINE_ bool is_disabled_collisions_between_bodies() const { return disabled_collisions_between_bodies; }
virtual bool setup(real_t p_step) = 0; virtual bool setup(real_t p_step) = 0;
virtual bool pre_solve(real_t p_step) = 0;
virtual void solve(real_t p_step) = 0; virtual void solve(real_t p_step) = 0;
virtual ~Constraint3DSW() {} virtual ~Constraint3DSW() {}

29
servers/physics_3d/joints/cone_twist_joint_3d_sw.cpp
View File

@ -109,7 +109,10 @@ ConeTwistJoint3DSW::ConeTwistJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Trans
} }
bool ConeTwistJoint3DSW::setup(real_t p_timestep) { bool ConeTwistJoint3DSW::setup(real_t p_timestep) {
if ((A->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC) && (B->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC)) { dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
if (!dynamic_A && !dynamic_B) {
return false; return false;
} }
@ -265,8 +268,12 @@ void ConeTwistJoint3DSW::solve(real_t p_timestep) {
real_t impulse = depth * tau / p_timestep * jacDiagABInv - rel_vel * jacDiagABInv; real_t impulse = depth * tau / p_timestep * jacDiagABInv - rel_vel * jacDiagABInv;
m_appliedImpulse += impulse; m_appliedImpulse += impulse;
Vector3 impulse_vector = normal * impulse; Vector3 impulse_vector = normal * impulse;
A->apply_impulse(impulse_vector, pivotAInW - A->get_transform().origin); if (dynamic_A) {
B->apply_impulse(-impulse_vector, pivotBInW - B->get_transform().origin); A->apply_impulse(impulse_vector, pivotAInW - A->get_transform().origin);
}
if (dynamic_B) {
B->apply_impulse(-impulse_vector, pivotBInW - B->get_transform().origin);
}
} }
} }
@ -287,8 +294,12 @@ void ConeTwistJoint3DSW::solve(real_t p_timestep) {
Vector3 impulse = m_swingAxis * impulseMag; Vector3 impulse = m_swingAxis * impulseMag;
A->apply_torque_impulse(impulse); if (dynamic_A) {
B->apply_torque_impulse(-impulse); A->apply_torque_impulse(impulse);
}
if (dynamic_B) {
B->apply_torque_impulse(-impulse);
}
} }
// solve twist limit // solve twist limit
@ -303,8 +314,12 @@ void ConeTwistJoint3DSW::solve(real_t p_timestep) {
Vector3 impulse = m_twistAxis * impulseMag; Vector3 impulse = m_twistAxis * impulseMag;
A->apply_torque_impulse(impulse); if (dynamic_A) {
B->apply_torque_impulse(-impulse); A->apply_torque_impulse(impulse);
}
if (dynamic_B) {
B->apply_torque_impulse(-impulse);
}
} }
} }
} }

6
servers/physics_3d/joints/cone_twist_joint_3d_sw.h
View File

@ -102,10 +102,10 @@ public:
bool m_solveSwingLimit; bool m_solveSwingLimit;
public: public:
virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_TYPE_CONE_TWIST; } virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_CONE_TWIST; }
virtual bool setup(real_t p_timestep); virtual bool setup(real_t p_step) override;
virtual void solve(real_t p_timestep); virtual void solve(real_t p_step) override;
ConeTwistJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform &rbAFrame, const Transform &rbBFrame); ConeTwistJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform &rbAFrame, const Transform &rbBFrame);

25
servers/physics_3d/joints/generic_6dof_joint_3d_sw.cpp
View File

@ -82,7 +82,7 @@ int G6DOFRotationalLimitMotor3DSW::testLimitValue(real_t test_value) {
real_t G6DOFRotationalLimitMotor3DSW::solveAngularLimits( real_t G6DOFRotationalLimitMotor3DSW::solveAngularLimits(
real_t timeStep, Vector3 &axis, real_t jacDiagABInv, real_t timeStep, Vector3 &axis, real_t jacDiagABInv,
Body3DSW *body0, Body3DSW *body1) { Body3DSW *body0, Body3DSW *body1, bool p_body0_dynamic, bool p_body1_dynamic) {
if (!needApplyTorques()) { if (!needApplyTorques()) {
return 0.0f; return 0.0f;
} }
@ -138,8 +138,10 @@ real_t G6DOFRotationalLimitMotor3DSW::solveAngularLimits(
Vector3 motorImp = clippedMotorImpulse * axis; Vector3 motorImp = clippedMotorImpulse * axis;
body0->apply_torque_impulse(motorImp); if (p_body0_dynamic) {
if (body1) { body0->apply_torque_impulse(motorImp);
}
if (body1 && p_body1_dynamic) {
body1->apply_torque_impulse(-motorImp); body1->apply_torque_impulse(-motorImp);
} }
@ -154,6 +156,7 @@ real_t G6DOFTranslationalLimitMotor3DSW::solveLinearAxis(
real_t jacDiagABInv, real_t jacDiagABInv,
Body3DSW *body1, const Vector3 &pointInA, Body3DSW *body1, const Vector3 &pointInA,
Body3DSW *body2, const Vector3 &pointInB, Body3DSW *body2, const Vector3 &pointInB,
bool p_body1_dynamic, bool p_body2_dynamic,
int limit_index, int limit_index,
const Vector3 &axis_normal_on_a, const Vector3 &axis_normal_on_a,
const Vector3 &anchorPos) { const Vector3 &anchorPos) {
@ -205,8 +208,12 @@ real_t G6DOFTranslationalLimitMotor3DSW::solveLinearAxis(
normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse; normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse;
Vector3 impulse_vector = axis_normal_on_a * normalImpulse; Vector3 impulse_vector = axis_normal_on_a * normalImpulse;
body1->apply_impulse(impulse_vector, rel_pos1); if (p_body1_dynamic) {
body2->apply_impulse(-impulse_vector, rel_pos2); body1->apply_impulse(impulse_vector, rel_pos1);
}
if (p_body2_dynamic) {
body2->apply_impulse(-impulse_vector, rel_pos2);
}
return normalImpulse; return normalImpulse;
} }
@ -303,7 +310,10 @@ bool Generic6DOFJoint3DSW::testAngularLimitMotor(int axis_index) {
} }
bool Generic6DOFJoint3DSW::setup(real_t p_timestep) { bool Generic6DOFJoint3DSW::setup(real_t p_timestep) {
if ((A->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC) && (B->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC)) { dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
if (!dynamic_A && !dynamic_B) {
return false; return false;
} }
@ -384,6 +394,7 @@ void Generic6DOFJoint3DSW::solve(real_t p_timestep) {
jacDiagABInv, jacDiagABInv,
A, pointInA, A, pointInA,
B, pointInB, B, pointInB,
dynamic_A, dynamic_B,
i, linear_axis, m_AnchorPos); i, linear_axis, m_AnchorPos);
} }
} }
@ -398,7 +409,7 @@ void Generic6DOFJoint3DSW::solve(real_t p_timestep) {
angularJacDiagABInv = real_t(1.) / m_jacAng[i].getDiagonal(); angularJacDiagABInv = real_t(1.) / m_jacAng[i].getDiagonal();
m_angularLimits[i].solveAngularLimits(m_timeStep, angular_axis, angularJacDiagABInv, A, B); m_angularLimits[i].solveAngularLimits(m_timeStep, angular_axis, angularJacDiagABInv, A, B, dynamic_A, dynamic_B);
} }
} }
} }

9
servers/physics_3d/joints/generic_6dof_joint_3d_sw.h
View File

@ -120,7 +120,7 @@ public:
int testLimitValue(real_t test_value); int testLimitValue(real_t test_value);
//! apply the correction impulses for two bodies //! apply the correction impulses for two bodies
real_t solveAngularLimits(real_t timeStep, Vector3 &axis, real_t jacDiagABInv, Body3DSW *body0, Body3DSW *body1); real_t solveAngularLimits(real_t timeStep, Vector3 &axis, real_t jacDiagABInv, Body3DSW *body0, Body3DSW *body1, bool p_body0_dynamic, bool p_body1_dynamic);
}; };
class G6DOFTranslationalLimitMotor3DSW { class G6DOFTranslationalLimitMotor3DSW {
@ -166,6 +166,7 @@ public:
real_t jacDiagABInv, real_t jacDiagABInv,
Body3DSW *body1, const Vector3 &pointInA, Body3DSW *body1, const Vector3 &pointInA,
Body3DSW *body2, const Vector3 &pointInB, Body3DSW *body2, const Vector3 &pointInB,
bool p_body1_dynamic, bool p_body2_dynamic,
int limit_index, int limit_index,
const Vector3 &axis_normal_on_a, const Vector3 &axis_normal_on_a,
const Vector3 &anchorPos); const Vector3 &anchorPos);
@ -234,10 +235,10 @@ protected:
public: public:
Generic6DOFJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform &frameInA, const Transform &frameInB, bool useLinearReferenceFrameA); Generic6DOFJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform &frameInA, const Transform &frameInB, bool useLinearReferenceFrameA);
virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_TYPE_6DOF; } virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_6DOF; }
virtual bool setup(real_t p_timestep); virtual bool setup(real_t p_step) override;
virtual void solve(real_t p_timestep); virtual void solve(real_t p_step) override;
//! Calcs global transform of the offsets //! Calcs global transform of the offsets
/*! /*!

37
servers/physics_3d/joints/hinge_joint_3d_sw.cpp
View File

@ -155,7 +155,10 @@ HingeJoint3DSW::HingeJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Vector3 &pivo
} }
bool HingeJoint3DSW::setup(real_t p_step) { bool HingeJoint3DSW::setup(real_t p_step) {
if ((A->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC) && (B->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC)) { dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
if (!dynamic_A && !dynamic_B) {
return false; return false;
} }
@ -279,8 +282,12 @@ void HingeJoint3DSW::solve(real_t p_step) {
real_t impulse = depth * tau / p_step * jacDiagABInv - rel_vel * jacDiagABInv; real_t impulse = depth * tau / p_step * jacDiagABInv - rel_vel * jacDiagABInv;
m_appliedImpulse += impulse; m_appliedImpulse += impulse;
Vector3 impulse_vector = normal * impulse; Vector3 impulse_vector = normal * impulse;
A->apply_impulse(impulse_vector, pivotAInW - A->get_transform().origin); if (dynamic_A) {
B->apply_impulse(-impulse_vector, pivotBInW - B->get_transform().origin); A->apply_impulse(impulse_vector, pivotAInW - A->get_transform().origin);
}
if (dynamic_B) {
B->apply_impulse(-impulse_vector, pivotBInW - B->get_transform().origin);
}
} }
} }
@ -322,8 +329,12 @@ void HingeJoint3DSW::solve(real_t p_step) {
angularError *= (real_t(1.) / denom2) * relaxation; angularError *= (real_t(1.) / denom2) * relaxation;
} }
A->apply_torque_impulse(-velrelOrthog + angularError); if (dynamic_A) {
B->apply_torque_impulse(velrelOrthog - angularError); A->apply_torque_impulse(-velrelOrthog + angularError);
}
if (dynamic_B) {
B->apply_torque_impulse(velrelOrthog - angularError);
}
// solve limit // solve limit
if (m_solveLimit) { if (m_solveLimit) {
@ -337,8 +348,12 @@ void HingeJoint3DSW::solve(real_t p_step) {
impulseMag = m_accLimitImpulse - temp; impulseMag = m_accLimitImpulse - temp;
Vector3 impulse = axisA * impulseMag * m_limitSign; Vector3 impulse = axisA * impulseMag * m_limitSign;
A->apply_torque_impulse(impulse); if (dynamic_A) {
B->apply_torque_impulse(-impulse); A->apply_torque_impulse(impulse);
}
if (dynamic_B) {
B->apply_torque_impulse(-impulse);
}
} }
} }
@ -359,8 +374,12 @@ void HingeJoint3DSW::solve(real_t p_step) {
clippedMotorImpulse = clippedMotorImpulse < -m_maxMotorImpulse ? -m_maxMotorImpulse : clippedMotorImpulse; clippedMotorImpulse = clippedMotorImpulse < -m_maxMotorImpulse ? -m_maxMotorImpulse : clippedMotorImpulse;
Vector3 motorImp = clippedMotorImpulse * axisA; Vector3 motorImp = clippedMotorImpulse * axisA;
A->apply_torque_impulse(motorImp + angularLimit); if (dynamic_A) {
B->apply_torque_impulse(-motorImp - angularLimit); A->apply_torque_impulse(motorImp + angularLimit);
}
if (dynamic_B) {
B->apply_torque_impulse(-motorImp - angularLimit);
}
} }
} }
} }

6
servers/physics_3d/joints/hinge_joint_3d_sw.h
View File

@ -96,10 +96,10 @@ class HingeJoint3DSW : public Joint3DSW {
real_t m_appliedImpulse; real_t m_appliedImpulse;
public: public:
virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_TYPE_HINGE; } virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_HINGE; }
virtual bool setup(real_t p_step); virtual bool setup(real_t p_step) override;
virtual void solve(real_t p_step); virtual void solve(real_t p_step) override;
real_t get_hinge_angle(); real_t get_hinge_angle();

13
servers/physics_3d/joints/pin_joint_3d_sw.cpp
View File

@ -50,7 +50,10 @@ subject to the following restrictions:
#include "pin_joint_3d_sw.h" #include "pin_joint_3d_sw.h"
bool PinJoint3DSW::setup(real_t p_step) { bool PinJoint3DSW::setup(real_t p_step) {
if ((A->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC) && (B->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC)) { dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
if (!dynamic_A && !dynamic_B) {
return false; return false;
} }
@ -123,8 +126,12 @@ void PinJoint3DSW::solve(real_t p_step) {
m_appliedImpulse += impulse; m_appliedImpulse += impulse;
Vector3 impulse_vector = normal * impulse; Vector3 impulse_vector = normal * impulse;
A->apply_impulse(impulse_vector, pivotAInW - A->get_transform().origin); if (dynamic_A) {
B->apply_impulse(-impulse_vector, pivotBInW - B->get_transform().origin); A->apply_impulse(impulse_vector, pivotAInW - A->get_transform().origin);
}
if (dynamic_B) {
B->apply_impulse(-impulse_vector, pivotBInW - B->get_transform().origin);
}
normal[i] = 0; normal[i] = 0;
} }

6
servers/physics_3d/joints/pin_joint_3d_sw.h
View File

@ -74,10 +74,10 @@ class PinJoint3DSW : public Joint3DSW {
Vector3 m_pivotInB; Vector3 m_pivotInB;
public: public:
virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_TYPE_PIN; } virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_PIN; }
virtual bool setup(real_t p_step); virtual bool setup(real_t p_step) override;
virtual void solve(real_t p_step); virtual void solve(real_t p_step) override;
void set_param(PhysicsServer3D::PinJointParam p_param, real_t p_value); void set_param(PhysicsServer3D::PinJointParam p_param, real_t p_value);
real_t get_param(PhysicsServer3D::PinJointParam p_param) const; real_t get_param(PhysicsServer3D::PinJointParam p_param) const;

45
servers/physics_3d/joints/slider_joint_3d_sw.cpp
View File

@ -127,7 +127,10 @@ SliderJoint3DSW::SliderJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform &
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
bool SliderJoint3DSW::setup(real_t p_step) { bool SliderJoint3DSW::setup(real_t p_step) {
if ((A->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC) && (B->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC)) { dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
if (!dynamic_A && !dynamic_B) {
return false; return false;
} }
@ -200,8 +203,12 @@ void SliderJoint3DSW::solve(real_t p_step) {
// calcutate and apply impulse // calcutate and apply impulse
real_t normalImpulse = softness * (restitution * depth / p_step - damping * rel_vel) * m_jacLinDiagABInv[i]; real_t normalImpulse = softness * (restitution * depth / p_step - damping * rel_vel) * m_jacLinDiagABInv[i];
Vector3 impulse_vector = normal * normalImpulse; Vector3 impulse_vector = normal * normalImpulse;
A->apply_impulse(impulse_vector, m_relPosA); if (dynamic_A) {
B->apply_impulse(-impulse_vector, m_relPosB); A->apply_impulse(impulse_vector, m_relPosA);
}
if (dynamic_B) {
B->apply_impulse(-impulse_vector, m_relPosB);
}
if (m_poweredLinMotor && (!i)) { // apply linear motor if (m_poweredLinMotor && (!i)) { // apply linear motor
if (m_accumulatedLinMotorImpulse < m_maxLinMotorForce) { if (m_accumulatedLinMotorImpulse < m_maxLinMotorForce) {
real_t desiredMotorVel = m_targetLinMotorVelocity; real_t desiredMotorVel = m_targetLinMotorVelocity;
@ -221,8 +228,12 @@ void SliderJoint3DSW::solve(real_t p_step) {
m_accumulatedLinMotorImpulse = new_acc; m_accumulatedLinMotorImpulse = new_acc;
// apply clamped impulse // apply clamped impulse
impulse_vector = normal * normalImpulse; impulse_vector = normal * normalImpulse;
A->apply_impulse(impulse_vector, m_relPosA); if (dynamic_A) {
B->apply_impulse(-impulse_vector, m_relPosB); A->apply_impulse(impulse_vector, m_relPosA);
}
if (dynamic_B) {
B->apply_impulse(-impulse_vector, m_relPosB);
}
} }
} }
} }
@ -256,8 +267,12 @@ void SliderJoint3DSW::solve(real_t p_step) {
angularError *= (real_t(1.) / denom2) * m_restitutionOrthoAng * m_softnessOrthoAng; angularError *= (real_t(1.) / denom2) * m_restitutionOrthoAng * m_softnessOrthoAng;
} }
// apply impulse // apply impulse
A->apply_torque_impulse(-velrelOrthog + angularError); if (dynamic_A) {
B->apply_torque_impulse(velrelOrthog - angularError); A->apply_torque_impulse(-velrelOrthog + angularError);
}
if (dynamic_B) {
B->apply_torque_impulse(velrelOrthog - angularError);
}
real_t impulseMag; real_t impulseMag;
//solve angular limits //solve angular limits
if (m_solveAngLim) { if (m_solveAngLim) {
@ -268,8 +283,12 @@ void SliderJoint3DSW::solve(real_t p_step) {
impulseMag *= m_kAngle * m_softnessDirAng; impulseMag *= m_kAngle * m_softnessDirAng;
} }
Vector3 impulse = axisA * impulseMag; Vector3 impulse = axisA * impulseMag;
A->apply_torque_impulse(impulse); if (dynamic_A) {
B->apply_torque_impulse(-impulse); A->apply_torque_impulse(impulse);
}
if (dynamic_B) {
B->apply_torque_impulse(-impulse);
}
//apply angular motor //apply angular motor
if (m_poweredAngMotor) { if (m_poweredAngMotor) {
if (m_accumulatedAngMotorImpulse < m_maxAngMotorForce) { if (m_accumulatedAngMotorImpulse < m_maxAngMotorForce) {
@ -294,8 +313,12 @@ void SliderJoint3DSW::solve(real_t p_step) {
m_accumulatedAngMotorImpulse = new_acc; m_accumulatedAngMotorImpulse = new_acc;
// apply clamped impulse // apply clamped impulse
Vector3 motorImp = angImpulse * axisA; Vector3 motorImp = angImpulse * axisA;
A->apply_torque_impulse(motorImp); if (dynamic_A) {
B->apply_torque_impulse(-motorImp); A->apply_torque_impulse(motorImp);
}
if (dynamic_B) {
B->apply_torque_impulse(-motorImp);
}
} }
} }
} // SliderJointSW::solveConstraint() } // SliderJointSW::solveConstraint()

6
servers/physics_3d/joints/slider_joint_3d_sw.h
View File

@ -240,10 +240,10 @@ public:
void set_param(PhysicsServer3D::SliderJointParam p_param, real_t p_value); void set_param(PhysicsServer3D::SliderJointParam p_param, real_t p_value);
real_t get_param(PhysicsServer3D::SliderJointParam p_param) const; real_t get_param(PhysicsServer3D::SliderJointParam p_param) const;
bool setup(real_t p_step); virtual bool setup(real_t p_step) override;
void solve(real_t p_step); virtual void solve(real_t p_step) override;
virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_TYPE_SLIDER; } virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_SLIDER; }
}; };
#endif // SLIDER_JOINT_SW_H #endif // SLIDER_JOINT_SW_H

9
servers/physics_3d/joints_3d_sw.h
View File

@ -35,9 +35,14 @@
#include "constraint_3d_sw.h" #include "constraint_3d_sw.h"
class Joint3DSW : public Constraint3DSW { class Joint3DSW : public Constraint3DSW {
protected:
bool dynamic_A = false;
bool dynamic_B = false;
public: public:
virtual bool setup(real_t p_step) { return false; } virtual bool setup(real_t p_step) override { return false; }
virtual void solve(real_t p_step) {} virtual bool pre_solve(real_t p_step) override { return true; }
virtual void solve(real_t p_step) override {}
void copy_settings_from(Joint3DSW *p_joint) { void copy_settings_from(Joint3DSW *p_joint) {
set_self(p_joint->get_self()); set_self(p_joint->get_self());

5
servers/physics_3d/soft_body_3d_sw.h
View File

@ -106,6 +106,8 @@ class SoftBody3DSW : public CollisionObject3DSW {
VSet<RID> exceptions; VSet<RID> exceptions;
uint64_t island_step = 0;
public: public:
SoftBody3DSW(); SoftBody3DSW();
@ -124,6 +126,9 @@ public:
_FORCE_INLINE_ bool has_exception(const RID &p_exception) const { return exceptions.has(p_exception); } _FORCE_INLINE_ bool has_exception(const RID &p_exception) const { return exceptions.has(p_exception); }
_FORCE_INLINE_ const VSet<RID> &get_exceptions() const { return exceptions; } _FORCE_INLINE_ const VSet<RID> &get_exceptions() const { return exceptions; }
_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; }
virtual void set_space(Space3DSW *p_space); virtual void set_space(Space3DSW *p_space);
void set_mesh(const Ref<Mesh> &p_mesh); void set_mesh(const Ref<Mesh> &p_mesh);

231
servers/physics_3d/step_3d_sw.cpp
View File

@ -37,39 +37,90 @@
#define BODY_ISLAND_SIZE_RESERVE 512 #define BODY_ISLAND_SIZE_RESERVE 512
#define ISLAND_COUNT_RESERVE 128 #define ISLAND_COUNT_RESERVE 128
#define ISLAND_SIZE_RESERVE 512 #define ISLAND_SIZE_RESERVE 512
#define CONSTRAINT_COUNT_RESERVE 1024
void Step3DSW::_populate_island(Body3DSW *p_body, LocalVector<Body3DSW *> &p_body_island, LocalVector<Constraint3DSW *> &p_constraint_island) { void Step3DSW::_populate_island(Body3DSW *p_body, LocalVector<Body3DSW *> &p_body_island, LocalVector<Constraint3DSW *> &p_constraint_island) {
p_body->set_island_step(_step); p_body->set_island_step(_step);
p_body_island.push_back(p_body);
// Faster with reversed iterations. if (p_body->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC) {
for (Map<Constraint3DSW *, int>::Element *E = p_body->get_constraint_map().back(); E; E = E->prev()) { // Only dynamic bodies are tested for activation.
Constraint3DSW *c = (Constraint3DSW *)E->key(); p_body_island.push_back(p_body);
if (c->get_island_step() == _step) { }
continue; //already processed
for (Map<Constraint3DSW *, int>::Element *E = p_body->get_constraint_map().front(); E; E = E->next()) {
Constraint3DSW *constraint = (Constraint3DSW *)E->key();
if (constraint->get_island_step() == _step) {
continue; // Already processed.
} }
c->set_island_step(_step); constraint->set_island_step(_step);
p_constraint_island.push_back(c); p_constraint_island.push_back(constraint);
for (int i = 0; i < c->get_body_count(); i++) { all_constraints.push_back(constraint);
// Find connected rigid bodies.
for (int i = 0; i < constraint->get_body_count(); i++) {
if (i == E->get()) { if (i == E->get()) {
continue; continue;
} }
Body3DSW *b = c->get_body_ptr()[i]; Body3DSW *other_body = constraint->get_body_ptr()[i];
if (b->get_island_step() == _step || b->get_mode() == PhysicsServer3D::BODY_MODE_STATIC || b->get_mode() == PhysicsServer3D::BODY_MODE_KINEMATIC) { if (other_body->get_island_step() == _step) {
continue; //no go continue; // Already processed.
} }
_populate_island(c->get_body_ptr()[i], p_body_island, p_constraint_island); if (other_body->get_mode() == PhysicsServer3D::BODY_MODE_STATIC) {
continue; // Static bodies don't connect islands.
}
_populate_island(other_body, p_body_island, p_constraint_island);
}
// Find connected soft bodies.
for (int i = 0; i < constraint->get_soft_body_count(); i++) {
SoftBody3DSW *soft_body = constraint->get_soft_body_ptr(i);
if (soft_body->get_island_step() == _step) {
continue; // Already processed.
}
_populate_island_soft_body(soft_body, p_body_island, p_constraint_island);
} }
} }
} }
void Step3DSW::_setup_island(LocalVector<Constraint3DSW *> &p_constraint_island, real_t p_delta) { void Step3DSW::_populate_island_soft_body(SoftBody3DSW *p_soft_body, LocalVector<Body3DSW *> &p_body_island, LocalVector<Constraint3DSW *> &p_constraint_island) {
p_soft_body->set_island_step(_step);
for (Set<Constraint3DSW *>::Element *E = p_soft_body->get_constraints().front(); E; E = E->next()) {
Constraint3DSW *constraint = (Constraint3DSW *)E->get();
if (constraint->get_island_step() == _step) {
continue; // Already processed.
}
constraint->set_island_step(_step);
p_constraint_island.push_back(constraint);
all_constraints.push_back(constraint);
// Find connected rigid bodies.
for (int i = 0; i < constraint->get_body_count(); i++) {
Body3DSW *body = constraint->get_body_ptr()[i];
if (body->get_island_step() == _step) {
continue; // Already processed.
}
if (body->get_mode() == PhysicsServer3D::BODY_MODE_STATIC) {
continue; // Static bodies don't connect islands.
}
_populate_island(body, p_body_island, p_constraint_island);
}
}
}
void Step3DSW::_setup_contraint(uint32_t p_constraint_index, void *p_userdata) {
Constraint3DSW *constraint = all_constraints[p_constraint_index];
constraint->setup(delta);
}
void Step3DSW::_pre_solve_island(LocalVector<Constraint3DSW *> &p_constraint_island) const {
uint32_t constraint_count = p_constraint_island.size(); uint32_t constraint_count = p_constraint_island.size();
uint32_t valid_constraint_count = 0; uint32_t valid_constraint_count = 0;
for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) { for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) {
Constraint3DSW *constraint = p_constraint_island[constraint_index]; Constraint3DSW *constraint = p_constraint_island[constraint_index];
if (p_constraint_island[constraint_index]->setup(p_delta)) { if (p_constraint_island[constraint_index]->pre_solve(delta)) {
// Keep this constraint for solving. // Keep this constraint for solving.
p_constraint_island[valid_constraint_count++] = constraint; p_constraint_island[valid_constraint_count++] = constraint;
} }
@ -77,15 +128,17 @@ void Step3DSW::_setup_island(LocalVector<Constraint3DSW *> &p_constraint_island,
p_constraint_island.resize(valid_constraint_count); p_constraint_island.resize(valid_constraint_count);
} }
void Step3DSW::_solve_island(LocalVector<Constraint3DSW *> &p_constraint_island, int p_iterations, real_t p_delta) { void Step3DSW::_solve_island(uint32_t p_island_index, void *p_userdata) {
LocalVector<Constraint3DSW *> &constraint_island = constraint_islands[p_island_index];
int current_priority = 1; int current_priority = 1;
uint32_t constraint_count = p_constraint_island.size(); uint32_t constraint_count = constraint_island.size();
while (constraint_count > 0) { while (constraint_count > 0) {
for (int i = 0; i < p_iterations; i++) { for (int i = 0; i < iterations; i++) {
// Go through all iterations. // Go through all iterations.
for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) { for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) {
p_constraint_island[constraint_index]->solve(p_delta); constraint_island[constraint_index]->solve(delta);
} }
} }
@ -93,28 +146,24 @@ void Step3DSW::_solve_island(LocalVector<Constraint3DSW *> &p_constraint_island,
uint32_t priority_constraint_count = 0; uint32_t priority_constraint_count = 0;
++current_priority; ++current_priority;
for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) { for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) {
Constraint3DSW *constraint = p_constraint_island[constraint_index]; Constraint3DSW *constraint = constraint_island[constraint_index];
if (constraint->get_priority() >= current_priority) { if (constraint->get_priority() >= current_priority) {
// Keep this constraint for the next iteration. // Keep this constraint for the next iteration.
p_constraint_island[priority_constraint_count++] = constraint; constraint_island[priority_constraint_count++] = constraint;
} }
} }
constraint_count = priority_constraint_count; constraint_count = priority_constraint_count;
} }
} }
void Step3DSW::_check_suspend(const LocalVector<Body3DSW *> &p_body_island, real_t p_delta) { void Step3DSW::_check_suspend(const LocalVector<Body3DSW *> &p_body_island) const {
bool can_sleep = true; bool can_sleep = true;
uint32_t body_count = p_body_island.size(); uint32_t body_count = p_body_island.size();
for (uint32_t body_index = 0; body_index < body_count; ++body_index) { for (uint32_t body_index = 0; body_index < body_count; ++body_index) {
Body3DSW *body = p_body_island[body_index]; Body3DSW *body = p_body_island[body_index];
if (body->get_mode() == PhysicsServer3D::BODY_MODE_STATIC || body->get_mode() == PhysicsServer3D::BODY_MODE_KINEMATIC) { if (!body->sleep_test(delta)) {
continue; // Ignore for static.
}
if (!body->sleep_test(p_delta)) {
can_sleep = false; can_sleep = false;
} }
} }
@ -123,10 +172,6 @@ void Step3DSW::_check_suspend(const LocalVector<Body3DSW *> &p_body_island, real
for (uint32_t body_index = 0; body_index < body_count; ++body_index) { for (uint32_t body_index = 0; body_index < body_count; ++body_index) {
Body3DSW *body = p_body_island[body_index]; Body3DSW *body = p_body_island[body_index];
if (body->get_mode() == PhysicsServer3D::BODY_MODE_STATIC || body->get_mode() == PhysicsServer3D::BODY_MODE_KINEMATIC) {
continue; // Ignore for static.
}
bool active = body->is_active(); bool active = body->is_active();
if (active == can_sleep) { if (active == can_sleep) {
@ -140,6 +185,9 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) {
p_space->setup(); //update inertias, etc p_space->setup(); //update inertias, etc
iterations = p_iterations;
delta = p_delta;
const SelfList<Body3DSW>::List *body_list = &p_space->get_active_body_list(); const SelfList<Body3DSW>::List *body_list = &p_space->get_active_body_list();
const SelfList<SoftBody3DSW>::List *soft_body_list = &p_space->get_active_soft_body_list(); const SelfList<SoftBody3DSW>::List *soft_body_list = &p_space->get_active_soft_body_list();
@ -175,12 +223,39 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) {
profile_begtime = profile_endtime; profile_begtime = profile_endtime;
} }
/* GENERATE CONSTRAINT ISLANDS */ /* GENERATE CONSTRAINT ISLANDS FOR MOVING AREAS */
uint32_t island_count = 0;
const SelfList<Area3DSW>::List &aml = p_space->get_moved_area_list();
while (aml.first()) {
for (const Set<Constraint3DSW *>::Element *E = aml.first()->self()->get_constraints().front(); E; E = E->next()) {
Constraint3DSW *constraint = E->get();
if (constraint->get_island_step() == _step) {
continue;
}
constraint->set_island_step(_step);
// Each constraint can be on a separate island for areas as there's no solving phase.
++island_count;
if (constraint_islands.size() < island_count) {
constraint_islands.resize(island_count);
}
LocalVector<Constraint3DSW *> &constraint_island = constraint_islands[island_count - 1];
constraint_island.clear();
all_constraints.push_back(constraint);
constraint_island.push_back(constraint);
}
p_space->area_remove_from_moved_list((SelfList<Area3DSW> *)aml.first()); //faster to remove here
}
/* GENERATE CONSTRAINT ISLANDS FOR ACTIVE RIGID BODIES */
b = body_list->first(); b = body_list->first();
uint32_t body_island_count = 0; uint32_t body_island_count = 0;
uint32_t island_count = 0;
while (b) { while (b) {
Body3DSW *body = b->self(); Body3DSW *body = b->self();
@ -204,7 +279,9 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) {
_populate_island(body, body_island, constraint_island); _populate_island(body, body_island, constraint_island);
body_islands.push_back(body_island); if (body_island.is_empty()) {
--body_island_count;
}
if (constraint_island.is_empty()) { if (constraint_island.is_empty()) {
--island_count; --island_count;
@ -213,58 +290,54 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) {
b = b->next(); b = b->next();
} }
p_space->set_island_count((int)island_count); /* GENERATE CONSTRAINT ISLANDS FOR ACTIVE SOFT BODIES */
const SelfList<Area3DSW>::List &aml = p_space->get_moved_area_list();
while (aml.first()) {
for (const Set<Constraint3DSW *>::Element *E = aml.first()->self()->get_constraints().front(); E; E = E->next()) {
Constraint3DSW *c = E->get();
if (c->get_island_step() == _step) {
continue;
}
c->set_island_step(_step);
++island_count;
if (constraint_islands.size() < island_count) {
constraint_islands.resize(island_count);
}
LocalVector<Constraint3DSW *> &constraint_island = constraint_islands[island_count - 1];
constraint_island.clear();
constraint_island.push_back(c);
}
p_space->area_remove_from_moved_list((SelfList<Area3DSW> *)aml.first()); //faster to remove here
}
sb = soft_body_list->first(); sb = soft_body_list->first();
while (sb) { while (sb) {
for (const Set<Constraint3DSW *>::Element *E = sb->self()->get_constraints().front(); E; E = E->next()) { SoftBody3DSW *soft_body = sb->self();
Constraint3DSW *c = E->get();
if (c->get_island_step() == _step) { if (soft_body->get_island_step() != _step) {
continue; ++body_island_count;
if (body_islands.size() < body_island_count) {
body_islands.resize(body_island_count);
} }
c->set_island_step(_step); LocalVector<Body3DSW *> &body_island = body_islands[body_island_count - 1];
body_island.clear();
body_island.reserve(BODY_ISLAND_SIZE_RESERVE);
++island_count; ++island_count;
if (constraint_islands.size() < island_count) { if (constraint_islands.size() < island_count) {
constraint_islands.resize(island_count); constraint_islands.resize(island_count);
} }
LocalVector<Constraint3DSW *> &constraint_island = constraint_islands[island_count - 1]; LocalVector<Constraint3DSW *> &constraint_island = constraint_islands[island_count - 1];
constraint_island.clear(); constraint_island.clear();
constraint_island.push_back(c); constraint_island.reserve(ISLAND_SIZE_RESERVE);
_populate_island_soft_body(soft_body, body_island, constraint_island);
if (body_island.is_empty()) {
--body_island_count;
}
if (constraint_island.is_empty()) {
--island_count;
}
} }
sb = sb->next(); sb = sb->next();
} }
p_space->set_island_count((int)island_count);
{ //profile { //profile
profile_endtime = OS::get_singleton()->get_ticks_usec(); profile_endtime = OS::get_singleton()->get_ticks_usec();
p_space->set_elapsed_time(Space3DSW::ELAPSED_TIME_GENERATE_ISLANDS, profile_endtime - profile_begtime); p_space->set_elapsed_time(Space3DSW::ELAPSED_TIME_GENERATE_ISLANDS, profile_endtime - profile_begtime);
profile_begtime = profile_endtime; profile_begtime = profile_endtime;
} }
/* SETUP CONSTRAINT ISLANDS */ /* SETUP CONSTRAINTS / PROCESS COLLISIONS */
for (uint32_t island_index = 0; island_index < island_count; ++island_index) { uint32_t total_contraint_count = all_constraints.size();
_setup_island(constraint_islands[island_index], p_delta); work_pool.do_work(total_contraint_count, this, &Step3DSW::_setup_contraint, nullptr);
}
{ //profile { //profile
profile_endtime = OS::get_singleton()->get_ticks_usec(); profile_endtime = OS::get_singleton()->get_ticks_usec();
@ -272,12 +345,21 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) {
profile_begtime = profile_endtime; profile_begtime = profile_endtime;
} }
/* PRE-SOLVE CONSTRAINT ISLANDS */
// Warning: This doesn't run on threads, because it involves thread-unsafe processing.
for (uint32_t island_index = 0; island_index < island_count; ++island_index) {
_pre_solve_island(constraint_islands[island_index]);
}
/* SOLVE CONSTRAINT ISLANDS */ /* SOLVE CONSTRAINT ISLANDS */
for (uint32_t island_index = 0; island_index < island_count; ++island_index) { // Warning: _solve_island modifies the constraint islands for optimization purpose,
// Warning: _solve_island modifies the constraint islands for optimization purpose, // their content is not reliable after these calls and shouldn't be used anymore.
// their content is not reliable after these calls and shouldn't be used anymore. if (island_count > 1) {
_solve_island(constraint_islands[island_index], p_iterations, p_delta); work_pool.do_work(island_count, this, &Step3DSW::_solve_island, nullptr);
} else if (island_count > 0) {
_solve_island(0);
} }
{ //profile { //profile
@ -298,7 +380,7 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) {
/* SLEEP / WAKE UP ISLANDS */ /* SLEEP / WAKE UP ISLANDS */
for (uint32_t island_index = 0; island_index < body_island_count; ++island_index) { for (uint32_t island_index = 0; island_index < body_island_count; ++island_index) {
_check_suspend(body_islands[island_index], p_delta); _check_suspend(body_islands[island_index]);
} }
/* UPDATE SOFT BODY CONSTRAINTS */ /* UPDATE SOFT BODY CONSTRAINTS */
@ -315,6 +397,8 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) {
profile_begtime = profile_endtime; profile_begtime = profile_endtime;
} }
all_constraints.clear();
p_space->update(); p_space->update();
p_space->unlock(); p_space->unlock();
_step++; _step++;
@ -325,4 +409,11 @@ Step3DSW::Step3DSW() {
body_islands.reserve(BODY_ISLAND_COUNT_RESERVE); body_islands.reserve(BODY_ISLAND_COUNT_RESERVE);
constraint_islands.reserve(ISLAND_COUNT_RESERVE); constraint_islands.reserve(ISLAND_COUNT_RESERVE);
all_constraints.reserve(CONSTRAINT_COUNT_RESERVE);
work_pool.init();
}
Step3DSW::~Step3DSW() {
work_pool.finish();
} }

16
servers/physics_3d/step_3d_sw.h
View File

@ -34,21 +34,31 @@
#include "space_3d_sw.h" #include "space_3d_sw.h"
#include "core/templates/local_vector.h" #include "core/templates/local_vector.h"
#include "core/templates/thread_work_pool.h"
class Step3DSW { class Step3DSW {
uint64_t _step; uint64_t _step;
int iterations = 0;
real_t delta = 0.0;
ThreadWorkPool work_pool;
LocalVector<LocalVector<Body3DSW *>> body_islands; LocalVector<LocalVector<Body3DSW *>> body_islands;
LocalVector<LocalVector<Constraint3DSW *>> constraint_islands; LocalVector<LocalVector<Constraint3DSW *>> constraint_islands;
LocalVector<Constraint3DSW *> all_constraints;
void _populate_island(Body3DSW *p_body, LocalVector<Body3DSW *> &p_body_island, LocalVector<Constraint3DSW *> &p_constraint_island); void _populate_island(Body3DSW *p_body, LocalVector<Body3DSW *> &p_body_island, LocalVector<Constraint3DSW *> &p_constraint_island);
void _setup_island(LocalVector<Constraint3DSW *> &p_constraint_island, real_t p_delta); void _populate_island_soft_body(SoftBody3DSW *p_soft_body, LocalVector<Body3DSW *> &p_body_island, LocalVector<Constraint3DSW *> &p_constraint_island);
void _solve_island(LocalVector<Constraint3DSW *> &p_constraint_island, int p_iterations, real_t p_delta); void _setup_contraint(uint32_t p_constraint_index, void *p_userdata = nullptr);
void _check_suspend(const LocalVector<Body3DSW *> &p_body_island, real_t p_delta); void _pre_solve_island(LocalVector<Constraint3DSW *> &p_constraint_island) const;
void _solve_island(uint32_t p_island_index, void *p_userdata = nullptr);
void _check_suspend(const LocalVector<Body3DSW *> &p_body_island) const;
public: public:
void step(Space3DSW *p_space, real_t p_delta, int p_iterations); void step(Space3DSW *p_space, real_t p_delta, int p_iterations);
Step3DSW(); Step3DSW();
~Step3DSW();
}; };
#endif // STEP__SW_H #endif // STEP__SW_H