mirror/godot
2
0
Fork 0
mirror of https://github.com/godotengine/godot.git synced 2024-11-27 09:16:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Godot Physics collisions and solver processed on threads

Browse Source 
Use ThreadWorkPool to process physics step tasks in multiple threads. Collisions are all processed in parallel and solving impulses is
processed in parallel for rigid body islands.

Additional changes:
- Proper islands for soft bodies linked to active bodies
- All moving areas are on separate islands (can be parallelized)
- Fix inconsistencies with body islands (Kinematic bodies could link
bodies together or not depending on the processing order)
- Completely prevent static bodies to be active (it could cause islands
to be wrongly created and cause dangerous multi-threading operations as
well as inconsistencies in created islands)
- Apply impulses only on dynamic bodies to avoid unsafe multi-threaded
operations (static bodies can be on multiple islands)
- Removed inverted iterations when populating body islands, it's now
faster in regular order (maybe after fixing inconsistencies)
This commit is contained in:
PouleyKetchoupp 2021-04-19 18:38:11 -07:00
parent 639b02f454
commit 448c41a3e4
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;
}
process_collision = false;
if (result != colliding) {
if (result) {
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);
}
if (area->get_space_override_mode() != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) {
process_collision = true;
} else if (area->has_monitor_callback()) {
process_collision = true;
}
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) {
// Nothing to do.
}
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;
body_shape = p_body_shape;
area_shape = p_area_shape;
colliding = false;
body->add_constraint(this, 0);
area->add_constraint(this);
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;
}
process_collision = false;
if (result != colliding) {
if (result) {
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);
}
if (area_b->has_area_monitor_callback() && area_a->is_monitorable()) {
process_collision = true;
} else if (area_a->has_area_monitor_callback() && area_b->is_monitorable()) {
process_collision = true;
}
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) {
// Nothing to do.
}
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;
shape_a = p_shape_a;
shape_b = p_shape_b;
colliding = false;
area_a->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"
class AreaPair2DSW : public Constraint2DSW {
Body2DSW *body;
Area2DSW *area;
int body_shape;
int area_shape;
bool colliding;
Body2DSW *body = nullptr;
Area2DSW *area = nullptr;
int body_shape = 0;
int area_shape = 0;
bool colliding = false;
bool process_collision = false;
public:
bool setup(real_t p_step);
void solve(real_t p_step);
virtual bool setup(real_t p_step) override;
virtual bool pre_solve(real_t p_step) override;
virtual void solve(real_t p_step) override;
AreaPair2DSW(Body2DSW *p_body, int p_body_shape, Area2DSW *p_area, int p_area_shape);
~AreaPair2DSW();
};
class Area2Pair2DSW : public Constraint2DSW {
Area2DSW *area_a;
Area2DSW *area_b;
int shape_a;
int shape_b;
bool colliding;
Area2DSW *area_a = nullptr;
Area2DSW *area_b = nullptr;
int shape_a = 0;
int shape_b = 0;
bool colliding = false;
bool process_collision = false;
public:
bool setup(real_t p_step);
void solve(real_t p_step);
virtual bool setup(real_t p_step) override;
virtual bool pre_solve(real_t p_step) override;
virtual void solve(real_t p_step) override;
Area2Pair2DSW(Area2DSW *p_area_a, int p_shape_a, Area2DSW *p_area_b, int p_shape_b);
~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;
if (!p_active) {
if (get_space()) {
get_space()->body_remove_from_active_list(&active_list);
}
} else {
if (active) {
if (mode == PhysicsServer2D::BODY_MODE_STATIC) {
return; //static bodies can't become active
}
if (get_space()) {
// Static bodies can't be active.
active = false;
} else if (get_space()) {
get_space()->body_add_to_active_list(&active_list);
}
//still_time=0;
} else if (get_space()) {
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) {
@ -370,13 +356,6 @@ void Body2DSW::set_space(Space2DSW *p_space) {
if (active) {
get_space()->body_add_to_active_list(&active_list);
}
/*
_update_queries();
if (is_active()) {
active=false;
set_active(true);
}
*/
}
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;
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++) {
Contact &c = (i == contact_count) ? contact : contacts[i];
Vector2 global_A = A->get_transform().basis_xform(c.local_A);
Vector2 global_B = B->get_transform().basis_xform(c.local_B) + offset_B;
Vector2 global_A = transform_A.basis_xform(c.local_A);
Vector2 global_B = transform_B.basis_xform(c.local_B) + offset_B;
Vector2 axis = global_A - global_B;
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_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++) {
Contact &c = contacts[i];
@ -134,8 +140,8 @@ void BodyPair2DSW::_validate_contacts() {
} else {
c.reused = false;
Vector2 global_A = A->get_transform().basis_xform(c.local_A);
Vector2 global_B = B->get_transform().basis_xform(c.local_B) + offset_B;
Vector2 global_A = transform_A.basis_xform(c.local_A);
Vector2 global_B = transform_B.basis_xform(c.local_B) + offset_B;
Vector2 axis = global_A - global_B;
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) {
//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())) {
collided = false;
return false;
}
bool report_contacts_only = false;
if ((A->get_mode() <= PhysicsServer2D::BODY_MODE_KINEMATIC) && (B->get_mode() <= PhysicsServer2D::BODY_MODE_KINEMATIC)) {
report_contacts_only = false;
if (!dynamic_A && !dynamic_B) {
if ((A->get_max_contacts_reported() > 0) || (B->get_max_contacts_reported() > 0)) {
report_contacts_only = true;
} else {
@ -246,12 +254,12 @@ bool BodyPair2DSW::setup(real_t p_step) {
_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_A = xform_Au * A->get_shape_transform(shape_A);
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);
Shape2DSW *shape_A_ptr = A->get_shape(shape_A);
@ -272,13 +280,13 @@ bool BodyPair2DSW::setup(real_t p_step) {
if (!collided) {
//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)) {
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)) {
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 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() == 0) {
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;
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++) {
Contact &c = contacts[i];
c.active = false;
Vector2 global_A = xform_Au.xform(c.local_A);
Vector2 global_B = xform_Bu.xform(c.local_B);
Vector2 global_A = transform_A.basis_xform(c.local_A);
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) {
continue;
@ -396,8 +418,6 @@ bool BodyPair2DSW::setup(real_t p_step) {
continue;
}
c.active = true;
// Precompute normal mass, tangent mass, and bias.
real_t rnA = c.rA.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
Vector2 P = c.acc_normal_impulse * c.normal + c.acc_tangent_impulse * tangent;
A->apply_impulse(-P, c.rA);
B->apply_impulse(P, c.rB);
if (dynamic_A) {
A->apply_impulse(-P, c.rA);
}
if (dynamic_B) {
B->apply_impulse(P, c.rB);
}
}
#endif
@ -434,6 +458,7 @@ bool BodyPair2DSW::setup(real_t p_step) {
c.bounce = c.bounce * dv.dot(c.normal);
}
c.active = true;
do_process = true;
}
@ -441,13 +466,12 @@ bool BodyPair2DSW::setup(real_t p_step) {
}
void BodyPair2DSW::solve(real_t p_step) {
if (!collided) {
if (!collided || oneway_disabled) {
return;
}
for (int i = 0; i < contact_count; ++i) {
Contact &c = contacts[i];
cc++;
if (!c.active) {
continue;
@ -474,8 +498,12 @@ void BodyPair2DSW::solve(real_t p_step) {
Vector2 jb = c.normal * (c.acc_bias_impulse - jbnOld);
A->apply_bias_impulse(-jb, c.rA);
B->apply_bias_impulse(jb, c.rB);
if (dynamic_A) {
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 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);
A->apply_impulse(-j, c.rA);
B->apply_impulse(j, c.rB);
if (dynamic_A) {
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();
A->add_constraint(this, 0);
B->add_constraint(this, 1);
contact_count = 0;
collided = false;
oneway_disabled = false;
}
BodyPair2DSW::~BodyPair2DSW() {

24
servers/physics_2d/body_pair_2d_sw.h
View File

@ -44,13 +44,16 @@ class BodyPair2DSW : public Constraint2DSW {
Body2DSW *B;
};
Body2DSW *_arr[2];
Body2DSW *_arr[2] = { nullptr, nullptr };
};
int shape_A;
int shape_B;
int shape_A = 0;
int shape_B = 0;
Space2DSW *space;
bool dynamic_A = false;
bool dynamic_B = false;
Space2DSW *space = nullptr;
struct Contact {
Vector2 position;
@ -73,10 +76,10 @@ class BodyPair2DSW : public Constraint2DSW {
Vector2 sep_axis;
Contact contacts[MAX_CONTACTS];
int contact_count;
bool collided;
bool oneway_disabled;
int cc;
int contact_count = 0;
bool collided = false;
bool oneway_disabled = false;
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);
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);
public:
bool setup(real_t p_step);
void solve(real_t p_step);
virtual bool setup(real_t p_step) override;
virtual bool pre_solve(real_t p_step) override;
virtual void solve(real_t p_step) override;
BodyPair2DSW(Body2DSW *p_A, int p_shape_A, Body2DSW *p_B, int p_shape_B);
~BodyPair2DSW();

4
servers/physics_2d/collision_object_2d_sw.h
View File

@ -143,8 +143,8 @@ public:
return shapes[p_index].metadata;
}
_FORCE_INLINE_ Transform2D get_transform() const { return transform; }
_FORCE_INLINE_ Transform2D get_inv_transform() const { return inv_transform; }
_FORCE_INLINE_ const Transform2D &get_transform() const { return transform; }
_FORCE_INLINE_ const Transform2D &get_inv_transform() const { return inv_transform; }
_FORCE_INLINE_ Space2DSW *get_space() const { return space; }
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; }
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 ~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) {
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;
}
@ -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);
// apply accumulated impulse
A->apply_impulse(-P, rA);
if (B) {
B->apply_impulse(P, rB);
}
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);
}
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) {
// compute relative 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);
A->apply_impulse(-impulse, rA);
if (B) {
if (dynamic_A) {
A->apply_impulse(-impulse, rA);
}
if (B && dynamic_B) {
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) {
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;
}
Space2DSW *space = A->get_space();
ERR_FAIL_COND_V(!space, false);
// calculate endpoints in worldspace
Vector2 ta = A->get_transform().xform(A_groove_1);
Vector2 tb = A->get_transform().xform(A_groove_2);
Space2DSW *space = A->get_space();
// calculate axis
Vector2 n = -(tb - ta).orthogonal().normalized();
@ -308,14 +324,22 @@ bool GrooveJoint2DSW::setup(real_t p_step) {
real_t _b = get_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;
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) {
// compute impulse
Vector2 vr = relative_velocity(A, B, rA, rB);
@ -328,8 +352,12 @@ void GrooveJoint2DSW::solve(real_t p_step) {
j = jn_acc - jOld;
A->apply_impulse(-j, rA);
B->apply_impulse(j, rB);
if (dynamic_A) {
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) :
@ -351,7 +379,10 @@ GrooveJoint2DSW::GrooveJoint2DSW(const Vector2 &p_a_groove1, const Vector2 &p_a_
//////////////////////////////////////////////
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;
}
@ -373,12 +404,21 @@ bool DampedSpringJoint2DSW::setup(real_t p_step) {
target_vrn = 0.0f;
v_coef = 1.0f - Math::exp(-damping * (p_step)*k);
// apply spring force
// Calculate spring force.
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);
B->apply_impulse(j, rB);
return true;
}
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;
}
@ -393,8 +433,12 @@ void DampedSpringJoint2DSW::solve(real_t p_step) {
target_vrn = vrn + v_damp;
Vector2 j = n * v_damp * n_mass;
A->apply_impulse(-j, rA);
B->apply_impulse(j, rB);
if (dynamic_A) {
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) {

31
servers/physics_2d/joints_2d_sw.h
View File

@ -39,6 +39,10 @@ class Joint2DSW : public Constraint2DSW {
real_t bias;
real_t max_bias;
protected:
bool dynamic_A = false;
bool dynamic_B = false;
public:
_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; }
@ -49,8 +53,9 @@ public:
_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; }
virtual bool setup(real_t p_step) { return false; }
virtual void solve(real_t p_step) {}
virtual bool setup(real_t p_step) override { return false; }
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);
@ -90,10 +95,11 @@ class PinJoint2DSW : public Joint2DSW {
real_t softness;
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 void solve(real_t p_step);
virtual bool setup(real_t p_step) override;
virtual bool pre_solve(real_t p_step) override;
virtual void solve(real_t p_step) override;
void set_param(PhysicsServer2D::PinJointParam p_param, real_t p_value);
real_t get_param(PhysicsServer2D::PinJointParam p_param) const;
@ -126,10 +132,11 @@ class GrooveJoint2DSW : public Joint2DSW {
bool correct;
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 void solve(real_t p_step);
virtual bool setup(real_t p_step) override;
virtual bool pre_solve(real_t p_step) override;
virtual void solve(real_t p_step) override;
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 n;
Vector2 j;
real_t n_mass;
real_t target_vrn;
real_t v_coef;
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 void solve(real_t p_step);
virtual bool setup(real_t p_step) override;
virtual bool pre_solve(real_t p_step) override;
virtual void solve(real_t p_step) override;
void set_param(PhysicsServer2D::DampedSpringParam p_param, real_t p_value);
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 "core/os/os.h"
#define BODY_ISLAND_COUNT_RESERVE 128
#define BODY_ISLAND_SIZE_RESERVE 512
#define ISLAND_COUNT_RESERVE 128
#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) {
p_body->set_island_step(_step);
p_body_island.push_back(p_body);
// Faster with reversed iterations.
for (const List<Pair<Constraint2DSW *, int>>::Element *E = p_body->get_constraint_list().back(); E; E = E->prev()) {
Constraint2DSW *c = (Constraint2DSW *)E->get().first;
if (c->get_island_step() == _step) {
continue; //already processed
if (p_body->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC) {
// Only dynamic bodies are tested for activation.
p_body_island.push_back(p_body);
}
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);
p_constraint_island.push_back(c);
constraint->set_island_step(_step);
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) {
continue;
}
Body2DSW *b = c->get_body_ptr()[i];
if (b->get_island_step() == _step || b->get_mode() == PhysicsServer2D::BODY_MODE_STATIC || b->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC) {
continue; //no go
Body2DSW *other_body = constraint->get_body_ptr()[i];
if (other_body->get_island_step() == _step) {
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 valid_constraint_count = 0;
for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++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.
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);
}
void Step2DSW::_solve_island(LocalVector<Constraint2DSW *> &p_constraint_island, int p_iterations, real_t p_delta) {
for (int i = 0; i < p_iterations; i++) {
uint32_t constraint_count = p_constraint_island.size();
void Step2DSW::_solve_island(uint32_t p_island_index, void *p_userdata) const {
const LocalVector<Constraint2DSW *> &constraint_island = constraint_islands[p_island_index];
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) {
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;
uint32_t body_count = p_body_island.size();
for (uint32_t body_index = 0; body_index < body_count; ++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.
}
if (!body->sleep_test(p_delta)) {
if (!body->sleep_test(delta)) {
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) {
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();
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
iterations = p_iterations;
delta = p_delta;
const SelfList<Body2DSW>::List *body_list = &p_space->get_active_body_list();
/* INTEGRATE FORCES */
@ -145,12 +156,39 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) {
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();
uint32_t body_island_count = 0;
uint32_t island_count = 0;
while (b) {
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);
body_islands.push_back(body_island);
if (body_island.is_empty()) {
--body_island_count;
}
if (constraint_island.is_empty()) {
--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);
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_endtime = OS::get_singleton()->get_ticks_usec();
p_space->set_elapsed_time(Space2DSW::ELAPSED_TIME_GENERATE_ISLANDS, profile_endtime - profile_begtime);
profile_begtime = profile_endtime;
}
/* SETUP CONSTRAINT ISLANDS */
/* SETUP CONSTRAINTS / PROCESS COLLISIONS */
for (uint32_t island_index = 0; island_index < island_count; ++island_index) {
_setup_island(constraint_islands[island_index], p_delta);
}
uint32_t total_contraint_count = all_constraints.size();
work_pool.do_work(total_contraint_count, this, &Step2DSW::_setup_contraint, nullptr);
{ //profile
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;
}
/* 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 */
for (uint32_t island_index = 0; island_index < island_count; ++island_index) {
_solve_island(constraint_islands[island_index], p_iterations, p_delta);
// Warning: _solve_island modifies the constraint islands for optimization purpose,
// 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
@ -247,7 +277,7 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) {
/* SLEEP / WAKE UP ISLANDS */
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
@ -256,6 +286,8 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) {
//profile_begtime=profile_endtime;
}
all_constraints.clear();
p_space->update();
p_space->unlock();
_step++;
@ -266,4 +298,11 @@ Step2DSW::Step2DSW() {
body_islands.reserve(BODY_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 "core/templates/local_vector.h"
#include "core/templates/thread_work_pool.h"
class Step2DSW {
uint64_t _step;
int iterations = 0;
real_t delta = 0.0;
ThreadWorkPool work_pool;
LocalVector<LocalVector<Body2DSW *>> body_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 _setup_island(LocalVector<Constraint2DSW *> &p_constraint_island, real_t p_delta);
void _solve_island(LocalVector<Constraint2DSW *> &p_constraint_island, int p_iterations, real_t p_delta);
void _check_suspend(const LocalVector<Body2DSW *> &p_body_island, real_t p_delta);
void _setup_contraint(uint32_t p_constraint_index, void *p_userdata = nullptr);
void _pre_solve_island(LocalVector<Constraint2DSW *> &p_constraint_island) const;
void _solve_island(uint32_t p_island_index, void *p_userdata = nullptr) const;
void _check_suspend(LocalVector<Body2DSW *> &p_body_island) const;
public:
void step(Space2DSW *p_space, real_t p_delta, int p_iterations);
Step2DSW();
~Step2DSW();
};
#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;
}
process_collision = false;
if (result != colliding) {
if (result) {
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);
}
if (area->get_space_override_mode() != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) {
process_collision = true;
} else if (area->has_monitor_callback()) {
process_collision = true;
}
colliding = result;
}
return 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) {
// Nothing to do.
}
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;
body_shape = p_body_shape;
area_shape = p_area_shape;
colliding = false;
body->add_constraint(this, 0);
area->add_constraint(this);
if (p_body->get_mode() == PhysicsServer3D::BODY_MODE_KINEMATIC) {
@ -103,33 +119,48 @@ bool Area2Pair3DSW::setup(real_t p_step) {
result = true;
}
process_collision = false;
if (result != colliding) {
if (result) {
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);
}
if (area_b->has_area_monitor_callback() && area_a->is_monitorable()) {
process_collision = true;
} else if (area_a->has_area_monitor_callback() && area_b->is_monitorable()) {
process_collision = true;
}
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) {
// Nothing to do.
}
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;
shape_a = p_shape_a;
shape_b = p_shape_b;
colliding = false;
area_a->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;
int body_shape;
int area_shape;
bool colliding;
bool colliding = false;
bool process_collision = false;
public:
bool setup(real_t p_step);
void solve(real_t p_step);
virtual bool setup(real_t p_step) override;
virtual bool pre_solve(real_t p_step) override;
virtual void solve(real_t p_step) override;
AreaPair3DSW(Body3DSW *p_body, int p_body_shape, Area3DSW *p_area, int p_area_shape);
~AreaPair3DSW();
@ -55,11 +57,13 @@ class Area2Pair3DSW : public Constraint3DSW {
Area3DSW *area_b;
int shape_a;
int shape_b;
bool colliding;
bool colliding = false;
bool process_collision = false;
public:
bool setup(real_t p_step);
void solve(real_t p_step);
virtual bool setup(real_t p_step) override;
virtual bool pre_solve(real_t p_step) override;
virtual void solve(real_t p_step) override;
Area2Pair3DSW(Area3DSW *p_area_a, int p_shape_a, Area3DSW *p_area_b, int p_shape_b);
~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;
if (!p_active) {
if (get_space()) {
get_space()->body_remove_from_active_list(&active_list);
}
} else {
if (active) {
if (mode == PhysicsServer3D::BODY_MODE_STATIC) {
return; //static bodies can't become active
}
if (get_space()) {
// Static bodies can't be active.
active = false;
} else if (get_space()) {
get_space()->body_add_to_active_list(&active_list);
}
//still_time=0;
} else if (get_space()) {
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) {
@ -392,13 +378,6 @@ void Body3DSW::set_space(Space3DSW *p_space) {
if (active) {
get_space()->body_add_to_active_list(&active_list);
}
/*
_update_queries();
if (is_active()) {
active=false;
set_active(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) {
//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())) {
collided = false;
return false;
}
bool report_contacts_only = false;
if ((A->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC) && (B->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC)) {
report_contacts_only = false;
if (!dynamic_A && !dynamic_B) {
if ((A->get_max_contacts_reported() > 0) || (B->get_max_contacts_reported() > 0)) {
report_contacts_only = true;
} else {
@ -237,7 +239,7 @@ bool BodyPair3DSW::setup(real_t p_step) {
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_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_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);
this->collided = collided;
collided = CollisionSolver3DSW::solve_static(shape_A_ptr, xform_A, shape_B_ptr, xform_B, _contact_added_callback, this, &sep_axis);
if (!collided) {
//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);
}
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);
}
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 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() == 0) {
bias = shape_B_ptr->get_custom_bias();
@ -283,22 +295,26 @@ bool BodyPair3DSW::setup(real_t p_step) {
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++) {
Contact &c = contacts[i];
c.active = false;
Vector3 global_A = xform_Au.xform(c.local_A);
Vector3 global_B = xform_Bu.xform(c.local_B);
Vector3 global_A = basis_A.xform(c.local_A);
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) {
continue;
}
#ifdef DEBUG_ENABLED
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_B + offset_A);
}
@ -338,8 +354,12 @@ bool BodyPair3DSW::setup(real_t p_step) {
c.depth = depth;
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());
B->apply_impulse(j_vec, c.rB + B->get_center_of_mass());
if (dynamic_A) {
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_center_of_mass = 0;
@ -361,6 +381,8 @@ void BodyPair3DSW::solve(real_t p_step) {
return;
}
const real_t max_bias_av = MAX_BIAS_ROTATION / p_step;
for (int i = 0; i < contact_count; i++) {
Contact &c = contacts[i];
if (!c.active) {
@ -384,8 +406,12 @@ void BodyPair3DSW::solve(real_t p_step) {
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);
B->apply_bias_impulse(jb, c.rB + B->get_center_of_mass(), MAX_BIAS_ROTATION / p_step);
if (dynamic_A) {
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);
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);
A->apply_bias_impulse(-jb_com, A->get_center_of_mass(), 0.0f);
B->apply_bias_impulse(jb_com, B->get_center_of_mass(), 0.0f);
if (dynamic_A) {
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;
@ -421,8 +451,12 @@ void BodyPair3DSW::solve(real_t p_step) {
Vector3 j = c.normal * (c.acc_normal_impulse - jnOld);
A->apply_impulse(-j, c.rA + A->get_center_of_mass());
B->apply_impulse(j, c.rB + B->get_center_of_mass());
if (dynamic_A) {
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;
}
@ -464,8 +498,12 @@ void BodyPair3DSW::solve(real_t p_step) {
jt = c.acc_tangent_impulse - jtOld;
A->apply_impulse(-jt, c.rA + A->get_center_of_mass());
B->apply_impulse(jt, c.rB + B->get_center_of_mass());
if (dynamic_A) {
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;
}
@ -481,8 +519,6 @@ BodyPair3DSW::BodyPair3DSW(Body3DSW *p_A, int p_shape_A, Body3DSW *p_B, int p_sh
space = A->get_space();
A->add_constraint(this, 0);
B->add_constraint(this, 1);
contact_count = 0;
collided = false;
}
BodyPair3DSW::~BodyPair3DSW() {
@ -564,6 +600,8 @@ void BodySoftBodyPair3DSW::validate_contacts() {
}
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())) {
collided = 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_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);
this->collided = collided;
collided = CollisionSolver3DSW::solve_static(shape_A_ptr, xform_A, shape_B_ptr, xform_B, _contact_added_callback, this, &sep_axis);
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 bias = (real_t)0.3;
Shape3DSW *shape_A_ptr = body->get_shape(body_shape);
if (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;
const Transform &transform_A = body->get_transform();
uint32_t contact_count = contacts.size();
for (uint32_t contact_index = 0; contact_index < contact_count; ++contact_index) {
Contact &c = contacts[contact_index];
@ -609,10 +659,10 @@ bool BodySoftBodyPair3DSW::setup(real_t p_step) {
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;
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) {
continue;
@ -629,7 +679,7 @@ bool BodySoftBodyPair3DSW::setup(real_t p_step) {
}
#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;
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);
}
if (body->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC) {
if (body_dynamic) {
body->set_active(true);
}
@ -651,7 +701,9 @@ bool BodySoftBodyPair3DSW::setup(real_t p_step) {
c.depth = depth;
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);
c.acc_bias_impulse = 0;
c.acc_bias_impulse_center_of_mass = 0;
@ -675,6 +727,8 @@ void BodySoftBodyPair3DSW::solve(real_t p_step) {
return;
}
const real_t max_bias_av = MAX_BIAS_ROTATION / p_step;
uint32_t contact_count = contacts.size();
for (uint32_t contact_index = 0; contact_index < contact_count; ++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);
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);
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);
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);
}
@ -732,7 +790,9 @@ void BodySoftBodyPair3DSW::solve(real_t p_step) {
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);
c.active = true;
@ -773,7 +833,9 @@ void BodySoftBodyPair3DSW::solve(real_t p_step) {
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);
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;
soft_body = p_B;
body_shape = p_shape_A;

38
servers/physics_3d/body_pair_3d_sw.h
View File

@ -57,9 +57,9 @@ protected:
};
Vector3 sep_axis;
bool collided;
bool collided = false;
Space3DSW *space;
Space3DSW *space = nullptr;
BodyContact3DSW(Body3DSW **p_body_ptr = nullptr, int p_body_count = 0) :
Constraint3DSW(p_body_ptr, p_body_count) {
@ -77,16 +77,21 @@ class BodyPair3DSW : public BodyContact3DSW {
Body3DSW *B;
};
Body3DSW *_arr[2];
Body3DSW *_arr[2] = { nullptr, nullptr };
};
int shape_A;
int shape_B;
int shape_A = 0;
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
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);
@ -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);
public:
bool setup(real_t p_step);
void solve(real_t p_step);
virtual bool setup(real_t p_step) override;
virtual bool pre_solve(real_t p_step) override;
virtual void solve(real_t p_step) override;
BodyPair3DSW(Body3DSW *p_A, int p_shape_A, Body3DSW *p_B, int p_shape_B);
~BodyPair3DSW();
};
class BodySoftBodyPair3DSW : public BodyContact3DSW {
Body3DSW *body;
SoftBody3DSW *soft_body;
Body3DSW *body = nullptr;
SoftBody3DSW *soft_body = nullptr;
int body_shape;
int body_shape = 0;
bool body_dynamic = false;
LocalVector<Contact> contacts;
@ -118,8 +126,12 @@ class BodySoftBodyPair3DSW : public BodyContact3DSW {
void validate_contacts();
public:
bool setup(real_t p_step);
void solve(real_t p_step);
virtual bool setup(real_t p_step) override;
virtual bool pre_solve(real_t p_step) override;
virtual void solve(real_t p_step) override;
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();

7
servers/physics_3d/constraint_3d_sw.h
View File

@ -31,7 +31,8 @@
#ifndef CONSTRAINT_SW_H
#define CONSTRAINT_SW_H
#include "body_3d_sw.h"
class Body3DSW;
class SoftBody3DSW;
class Constraint3DSW {
Body3DSW **_body_ptr;
@ -61,6 +62,9 @@ public:
_FORCE_INLINE_ Body3DSW **get_body_ptr() const { return _body_ptr; }
_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_ 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; }
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 ~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) {
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;
}
@ -265,8 +268,12 @@ void ConeTwistJoint3DSW::solve(real_t p_timestep) {
real_t impulse = depth * tau / p_timestep * jacDiagABInv - rel_vel * jacDiagABInv;
m_appliedImpulse += impulse;
Vector3 impulse_vector = normal * impulse;
A->apply_impulse(impulse_vector, pivotAInW - A->get_transform().origin);
B->apply_impulse(-impulse_vector, pivotBInW - B->get_transform().origin);
if (dynamic_A) {
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;
A->apply_torque_impulse(impulse);
B->apply_torque_impulse(-impulse);
if (dynamic_A) {
A->apply_torque_impulse(impulse);
}
if (dynamic_B) {
B->apply_torque_impulse(-impulse);
}
}
// solve twist limit
@ -303,8 +314,12 @@ void ConeTwistJoint3DSW::solve(real_t p_timestep) {
Vector3 impulse = m_twistAxis * impulseMag;
A->apply_torque_impulse(impulse);
B->apply_torque_impulse(-impulse);
if (dynamic_A) {
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;
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 void solve(real_t p_timestep);
virtual bool setup(real_t p_step) override;
virtual void solve(real_t p_step) override;
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 timeStep, Vector3 &axis, real_t jacDiagABInv,
Body3DSW *body0, Body3DSW *body1) {
Body3DSW *body0, Body3DSW *body1, bool p_body0_dynamic, bool p_body1_dynamic) {
if (!needApplyTorques()) {
return 0.0f;
}
@ -138,8 +138,10 @@ real_t G6DOFRotationalLimitMotor3DSW::solveAngularLimits(
Vector3 motorImp = clippedMotorImpulse * axis;
body0->apply_torque_impulse(motorImp);
if (body1) {
if (p_body0_dynamic) {
body0->apply_torque_impulse(motorImp);
}
if (body1 && p_body1_dynamic) {
body1->apply_torque_impulse(-motorImp);
}
@ -154,6 +156,7 @@ real_t G6DOFTranslationalLimitMotor3DSW::solveLinearAxis(
real_t jacDiagABInv,
Body3DSW *body1, const Vector3 &pointInA,
Body3DSW *body2, const Vector3 &pointInB,
bool p_body1_dynamic, bool p_body2_dynamic,
int limit_index,
const Vector3 &axis_normal_on_a,
const Vector3 &anchorPos) {
@ -205,8 +208,12 @@ real_t G6DOFTranslationalLimitMotor3DSW::solveLinearAxis(
normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse;
Vector3 impulse_vector = axis_normal_on_a * normalImpulse;
body1->apply_impulse(impulse_vector, rel_pos1);
body2->apply_impulse(-impulse_vector, rel_pos2);
if (p_body1_dynamic) {
body1->apply_impulse(impulse_vector, rel_pos1);
}
if (p_body2_dynamic) {
body2->apply_impulse(-impulse_vector, rel_pos2);
}
return normalImpulse;
}
@ -303,7 +310,10 @@ bool Generic6DOFJoint3DSW::testAngularLimitMotor(int axis_index) {
}
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;
}
@ -384,6 +394,7 @@ void Generic6DOFJoint3DSW::solve(real_t p_timestep) {
jacDiagABInv,
A, pointInA,
B, pointInB,
dynamic_A, dynamic_B,
i, linear_axis, m_AnchorPos);
}
}
@ -398,7 +409,7 @@ void Generic6DOFJoint3DSW::solve(real_t p_timestep) {
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);
//! 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 {
@ -166,6 +166,7 @@ public:
real_t jacDiagABInv,
Body3DSW *body1, const Vector3 &pointInA,
Body3DSW *body2, const Vector3 &pointInB,
bool p_body1_dynamic, bool p_body2_dynamic,
int limit_index,
const Vector3 &axis_normal_on_a,
const Vector3 &anchorPos);
@ -234,10 +235,10 @@ protected:
public:
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 void solve(real_t p_timestep);
virtual bool setup(real_t p_step) override;
virtual void solve(real_t p_step) override;
//! 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) {
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;
}
@ -279,8 +282,12 @@ void HingeJoint3DSW::solve(real_t p_step) {
real_t impulse = depth * tau / p_step * jacDiagABInv - rel_vel * jacDiagABInv;
m_appliedImpulse += impulse;
Vector3 impulse_vector = normal * impulse;
A->apply_impulse(impulse_vector, pivotAInW - A->get_transform().origin);
B->apply_impulse(-impulse_vector, pivotBInW - B->get_transform().origin);
if (dynamic_A) {
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;
}
A->apply_torque_impulse(-velrelOrthog + angularError);
B->apply_torque_impulse(velrelOrthog - angularError);
if (dynamic_A) {
A->apply_torque_impulse(-velrelOrthog + angularError);
}
if (dynamic_B) {
B->apply_torque_impulse(velrelOrthog - angularError);
}
// solve limit
if (m_solveLimit) {
@ -337,8 +348,12 @@ void HingeJoint3DSW::solve(real_t p_step) {
impulseMag = m_accLimitImpulse - temp;
Vector3 impulse = axisA * impulseMag * m_limitSign;
A->apply_torque_impulse(impulse);
B->apply_torque_impulse(-impulse);
if (dynamic_A) {
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;
Vector3 motorImp = clippedMotorImpulse * axisA;
A->apply_torque_impulse(motorImp + angularLimit);
B->apply_torque_impulse(-motorImp - angularLimit);
if (dynamic_A) {
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;
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 void solve(real_t p_step);
virtual bool setup(real_t p_step) override;
virtual void solve(real_t p_step) override;
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"
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;
}
@ -123,8 +126,12 @@ void PinJoint3DSW::solve(real_t p_step) {
m_appliedImpulse += impulse;
Vector3 impulse_vector = normal * impulse;
A->apply_impulse(impulse_vector, pivotAInW - A->get_transform().origin);
B->apply_impulse(-impulse_vector, pivotBInW - B->get_transform().origin);
if (dynamic_A) {
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;
}

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

@ -74,10 +74,10 @@ class PinJoint3DSW : public Joint3DSW {
Vector3 m_pivotInB;
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 void solve(real_t p_step);
virtual bool setup(real_t p_step) override;
virtual void solve(real_t p_step) override;
void set_param(PhysicsServer3D::PinJointParam p_param, real_t p_value);
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) {
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;
}
@ -200,8 +203,12 @@ void SliderJoint3DSW::solve(real_t p_step) {
// calcutate and apply impulse
real_t normalImpulse = softness * (restitution * depth / p_step - damping * rel_vel) * m_jacLinDiagABInv[i];
Vector3 impulse_vector = normal * normalImpulse;
A->apply_impulse(impulse_vector, m_relPosA);
B->apply_impulse(-impulse_vector, m_relPosB);
if (dynamic_A) {
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_accumulatedLinMotorImpulse < m_maxLinMotorForce) {
real_t desiredMotorVel = m_targetLinMotorVelocity;
@ -221,8 +228,12 @@ void SliderJoint3DSW::solve(real_t p_step) {
m_accumulatedLinMotorImpulse = new_acc;
// apply clamped impulse
impulse_vector = normal * normalImpulse;
A->apply_impulse(impulse_vector, m_relPosA);
B->apply_impulse(-impulse_vector, m_relPosB);
if (dynamic_A) {
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;
}
// apply impulse
A->apply_torque_impulse(-velrelOrthog + angularError);
B->apply_torque_impulse(velrelOrthog - angularError);
if (dynamic_A) {
A->apply_torque_impulse(-velrelOrthog + angularError);
}
if (dynamic_B) {
B->apply_torque_impulse(velrelOrthog - angularError);
}
real_t impulseMag;
//solve angular limits
if (m_solveAngLim) {
@ -268,8 +283,12 @@ void SliderJoint3DSW::solve(real_t p_step) {
impulseMag *= m_kAngle * m_softnessDirAng;
}
Vector3 impulse = axisA * impulseMag;
A->apply_torque_impulse(impulse);
B->apply_torque_impulse(-impulse);
if (dynamic_A) {
A->apply_torque_impulse(impulse);
}
if (dynamic_B) {
B->apply_torque_impulse(-impulse);
}
//apply angular motor
if (m_poweredAngMotor) {
if (m_accumulatedAngMotorImpulse < m_maxAngMotorForce) {
@ -294,8 +313,12 @@ void SliderJoint3DSW::solve(real_t p_step) {
m_accumulatedAngMotorImpulse = new_acc;
// apply clamped impulse
Vector3 motorImp = angImpulse * axisA;
A->apply_torque_impulse(motorImp);
B->apply_torque_impulse(-motorImp);
if (dynamic_A) {
A->apply_torque_impulse(motorImp);
}
if (dynamic_B) {
B->apply_torque_impulse(-motorImp);
}
}
}
} // 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);
real_t get_param(PhysicsServer3D::SliderJointParam p_param) const;
bool setup(real_t p_step);
void solve(real_t p_step);
virtual bool setup(real_t p_step) override;
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

9
servers/physics_3d/joints_3d_sw.h
View File

@ -35,9 +35,14 @@
#include "constraint_3d_sw.h"
class Joint3DSW : public Constraint3DSW {
protected:
bool dynamic_A = false;
bool dynamic_B = false;
public:
virtual bool setup(real_t p_step) { return false; }
virtual void solve(real_t p_step) {}
virtual bool setup(real_t p_step) override { return false; }
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) {
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;
uint64_t island_step = 0;
public:
SoftBody3DSW();
@ -124,6 +126,9 @@ public:
_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_ 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);
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 ISLAND_COUNT_RESERVE 128
#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) {
p_body->set_island_step(_step);
p_body_island.push_back(p_body);
// Faster with reversed iterations.
for (Map<Constraint3DSW *, int>::Element *E = p_body->get_constraint_map().back(); E; E = E->prev()) {
Constraint3DSW *c = (Constraint3DSW *)E->key();
if (c->get_island_step() == _step) {
continue; //already processed
if (p_body->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC) {
// Only dynamic bodies are tested for activation.
p_body_island.push_back(p_body);
}
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);
p_constraint_island.push_back(c);
constraint->set_island_step(_step);
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()) {
continue;
}
Body3DSW *b = c->get_body_ptr()[i];
if (b->get_island_step() == _step || b->get_mode() == PhysicsServer3D::BODY_MODE_STATIC || b->get_mode() == PhysicsServer3D::BODY_MODE_KINEMATIC) {
continue; //no go
Body3DSW *other_body = constraint->get_body_ptr()[i];
if (other_body->get_island_step() == _step) {
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 valid_constraint_count = 0;
for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++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.
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);
}
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;
uint32_t constraint_count = p_constraint_island.size();
uint32_t constraint_count = constraint_island.size();
while (constraint_count > 0) {
for (int i = 0; i < p_iterations; i++) {
for (int i = 0; i < iterations; i++) {
// Go through all iterations.
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;
++current_priority;
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) {
// 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;
}
}
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;
uint32_t body_count = p_body_island.size();
for (uint32_t body_index = 0; body_index < body_count; ++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.
}
if (!body->sleep_test(p_delta)) {
if (!body->sleep_test(delta)) {
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) {
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();
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
iterations = p_iterations;
delta = p_delta;
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();
@ -175,12 +223,39 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) {
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();
uint32_t body_island_count = 0;
uint32_t island_count = 0;
while (b) {
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);
body_islands.push_back(body_island);
if (body_island.is_empty()) {
--body_island_count;
}
if (constraint_island.is_empty()) {
--island_count;
@ -213,58 +290,54 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) {
b = b->next();
}
p_space->set_island_count((int)island_count);
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
}
/* GENERATE CONSTRAINT ISLANDS FOR ACTIVE SOFT BODIES */
sb = soft_body_list->first();
while (sb) {
for (const Set<Constraint3DSW *>::Element *E = sb->self()->get_constraints().front(); E; E = E->next()) {
Constraint3DSW *c = E->get();
if (c->get_island_step() == _step) {
continue;
SoftBody3DSW *soft_body = sb->self();
if (soft_body->get_island_step() != _step) {
++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;
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);
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();
}
p_space->set_island_count((int)island_count);
{ //profile
profile_endtime = OS::get_singleton()->get_ticks_usec();
p_space->set_elapsed_time(Space3DSW::ELAPSED_TIME_GENERATE_ISLANDS, profile_endtime - profile_begtime);
profile_begtime = profile_endtime;
}
/* SETUP CONSTRAINT ISLANDS */
/* SETUP CONSTRAINTS / PROCESS COLLISIONS */
for (uint32_t island_index = 0; island_index < island_count; ++island_index) {
_setup_island(constraint_islands[island_index], p_delta);
}
uint32_t total_contraint_count = all_constraints.size();
work_pool.do_work(total_contraint_count, this, &Step3DSW::_setup_contraint, nullptr);
{ //profile
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;
}
/* 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 */
for (uint32_t island_index = 0; island_index < island_count; ++island_index) {
// Warning: _solve_island modifies the constraint islands for optimization purpose,
// their content is not reliable after these calls and shouldn't be used anymore.
_solve_island(constraint_islands[island_index], p_iterations, p_delta);
// Warning: _solve_island modifies the constraint islands for optimization purpose,
// 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, &Step3DSW::_solve_island, nullptr);
} else if (island_count > 0) {
_solve_island(0);
}
{ //profile
@ -298,7 +380,7 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) {
/* SLEEP / WAKE UP ISLANDS */
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 */
@ -315,6 +397,8 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) {
profile_begtime = profile_endtime;
}
all_constraints.clear();
p_space->update();
p_space->unlock();
_step++;
@ -325,4 +409,11 @@ Step3DSW::Step3DSW() {
body_islands.reserve(BODY_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 "core/templates/local_vector.h"
#include "core/templates/thread_work_pool.h"
class Step3DSW {
uint64_t _step;
int iterations = 0;
real_t delta = 0.0;
ThreadWorkPool work_pool;
LocalVector<LocalVector<Body3DSW *>> body_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 _setup_island(LocalVector<Constraint3DSW *> &p_constraint_island, real_t p_delta);
void _solve_island(LocalVector<Constraint3DSW *> &p_constraint_island, int p_iterations, real_t p_delta);
void _check_suspend(const LocalVector<Body3DSW *> &p_body_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 _setup_contraint(uint32_t p_constraint_index, void *p_userdata = nullptr);
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:
void step(Space3DSW *p_space, real_t p_delta, int p_iterations);
Step3DSW();
~Step3DSW();
};
#endif // STEP__SW_H