summaryrefslogtreecommitdiff
path: root/servers/physics_3d
diff options
context:
space:
mode:
Diffstat (limited to 'servers/physics_3d')
-rw-r--r--servers/physics_3d/godot_body_pair_3d.cpp130
-rw-r--r--servers/physics_3d/godot_body_pair_3d.h1
-rw-r--r--servers/physics_3d/godot_physics_server_3d.cpp21
-rw-r--r--servers/physics_3d/godot_physics_server_3d.h3
-rw-r--r--servers/physics_3d/godot_space_3d.cpp14
-rw-r--r--servers/physics_3d/godot_space_3d.h5
-rw-r--r--servers/physics_3d/godot_step_3d.cpp4
-rw-r--r--servers/physics_3d/godot_step_3d.h2
8 files changed, 102 insertions, 78 deletions
diff --git a/servers/physics_3d/godot_body_pair_3d.cpp b/servers/physics_3d/godot_body_pair_3d.cpp
index 8a701466ed..5c25ba9537 100644
--- a/servers/physics_3d/godot_body_pair_3d.cpp
+++ b/servers/physics_3d/godot_body_pair_3d.cpp
@@ -44,11 +44,6 @@ void GodotBodyPair3D::_contact_added_callback(const Vector3 &p_point_A, int p_in
}
void GodotBodyPair3D::contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B) {
- // check if we already have the contact
-
- //Vector3 local_A = A->get_inv_transform().xform(p_point_A);
- //Vector3 local_B = B->get_inv_transform().xform(p_point_B);
-
Vector3 local_A = A->get_inv_transform().basis.xform(p_point_A);
Vector3 local_B = B->get_inv_transform().basis.xform(p_point_B - offset_B);
@@ -57,19 +52,14 @@ void GodotBodyPair3D::contact_added_callback(const Vector3 &p_point_A, int p_ind
ERR_FAIL_COND(new_index >= (MAX_CONTACTS + 1));
Contact contact;
-
- contact.acc_normal_impulse = 0;
- contact.acc_bias_impulse = 0;
- contact.acc_bias_impulse_center_of_mass = 0;
- contact.acc_tangent_impulse = Vector3();
contact.index_A = p_index_A;
contact.index_B = p_index_B;
contact.local_A = local_A;
contact.local_B = local_B;
contact.normal = (p_point_A - p_point_B).normalized();
- contact.mass_normal = 0; // will be computed in setup()
+ contact.used = true;
- // attempt to determine if the contact will be reused
+ // Attempt to determine if the contact will be reused.
real_t contact_recycle_radius = space->get_contact_recycle_radius();
for (int i = 0; i < contact_count; i++) {
@@ -80,23 +70,34 @@ void GodotBodyPair3D::contact_added_callback(const Vector3 &p_point_A, int p_ind
contact.acc_bias_impulse = c.acc_bias_impulse;
contact.acc_bias_impulse_center_of_mass = c.acc_bias_impulse_center_of_mass;
contact.acc_tangent_impulse = c.acc_tangent_impulse;
- new_index = i;
- break;
+ c = contact;
+ return;
}
}
- // figure out if the contact amount must be reduced to fit the new contact
-
+ // Figure out if the contact amount must be reduced to fit the new contact.
if (new_index == MAX_CONTACTS) {
- // remove the contact with the minimum depth
+ // Remove the contact with the minimum depth.
+
+ const Basis &basis_A = A->get_transform().basis;
+ const Basis &basis_B = B->get_transform().basis;
int least_deep = -1;
- real_t min_depth = 1e10;
+ real_t min_depth;
+
+ // Start with depth for new contact.
+ {
+ Vector3 global_A = basis_A.xform(contact.local_A);
+ Vector3 global_B = basis_B.xform(contact.local_B) + offset_B;
+
+ Vector3 axis = global_A - global_B;
+ min_depth = axis.dot(contact.normal);
+ }
- for (int i = 0; i <= contact_count; i++) {
- Contact &c = (i == contact_count) ? contact : contacts[i];
- Vector3 global_A = A->get_transform().basis.xform(c.local_A);
- Vector3 global_B = B->get_transform().basis.xform(c.local_B) + offset_B;
+ for (int i = 0; i < contact_count; i++) {
+ const Contact &c = contacts[i];
+ Vector3 global_A = basis_A.xform(c.local_A);
+ Vector3 global_B = basis_B.xform(c.local_B) + offset_B;
Vector3 axis = global_A - global_B;
real_t depth = axis.dot(c.normal);
@@ -107,10 +108,8 @@ void GodotBodyPair3D::contact_added_callback(const Vector3 &p_point_A, int p_ind
}
}
- ERR_FAIL_COND(least_deep == -1);
-
- if (least_deep < contact_count) { //replace the last deep contact by the new one
-
+ if (least_deep > -1) {
+ // Replace the least deep contact by the new one.
contacts[least_deep] = contact;
}
@@ -118,29 +117,41 @@ void GodotBodyPair3D::contact_added_callback(const Vector3 &p_point_A, int p_ind
}
contacts[new_index] = contact;
-
- if (new_index == contact_count) {
- contact_count++;
- }
+ contact_count++;
}
void GodotBodyPair3D::validate_contacts() {
- //make sure to erase contacts that are no longer valid
+ // Make sure to erase contacts that are no longer valid.
+ real_t max_separation = space->get_contact_max_separation();
+ real_t max_separation2 = max_separation * max_separation;
+
+ const Basis &basis_A = A->get_transform().basis;
+ const Basis &basis_B = B->get_transform().basis;
- real_t contact_max_separation = space->get_contact_max_separation();
for (int i = 0; i < contact_count; i++) {
Contact &c = contacts[i];
- Vector3 global_A = A->get_transform().basis.xform(c.local_A);
- Vector3 global_B = B->get_transform().basis.xform(c.local_B) + offset_B;
- Vector3 axis = global_A - global_B;
- real_t depth = axis.dot(c.normal);
+ bool erase = false;
+ if (!c.used) {
+ // Was left behind in previous frame.
+ erase = true;
+ } else {
+ c.used = false;
+
+ Vector3 global_A = basis_A.xform(c.local_A);
+ Vector3 global_B = basis_B.xform(c.local_B) + offset_B;
+ Vector3 axis = global_A - global_B;
+ real_t depth = axis.dot(c.normal);
- if (depth < -contact_max_separation || (global_B + c.normal * depth - global_A).length() > contact_max_separation) {
- // contact no longer needed, remove
+ if (depth < -max_separation || (global_B + c.normal * depth - global_A).length_squared() > max_separation2) {
+ erase = true;
+ }
+ }
+ if (erase) {
+ // Contact no longer needed, remove.
if ((i + 1) < contact_count) {
- // swap with the last one
+ // Swap with the last one.
SWAP(contacts[i], contacts[contact_count - 1]);
}
@@ -260,7 +271,7 @@ bool GodotBodyPair3D::pre_solve(real_t p_step) {
real_t max_penetration = space->get_contact_max_allowed_penetration();
- real_t bias = (real_t)0.3;
+ real_t bias = 0.8;
GodotShape3D *shape_A_ptr = A->get_shape(shape_A);
GodotShape3D *shape_B_ptr = B->get_shape(shape_B);
@@ -353,8 +364,6 @@ bool GodotBodyPair3D::pre_solve(real_t p_step) {
if (collide_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;
c.bounce = combine_bounce(A, B);
if (c.bounce) {
@@ -538,14 +547,10 @@ void GodotBodySoftBodyPair3D::contact_added_callback(const Vector3 &p_point_A, i
Contact contact;
contact.index_A = p_index_A;
contact.index_B = p_index_B;
- contact.acc_normal_impulse = 0;
- contact.acc_bias_impulse = 0;
- contact.acc_bias_impulse_center_of_mass = 0;
- contact.acc_tangent_impulse = Vector3();
contact.local_A = local_A;
contact.local_B = local_B;
contact.normal = (p_point_A - p_point_B).normalized();
- contact.mass_normal = 0;
+ contact.used = true;
// Attempt to determine if the contact will be reused.
real_t contact_recycle_radius = space->get_contact_recycle_radius();
@@ -571,20 +576,33 @@ void GodotBodySoftBodyPair3D::contact_added_callback(const Vector3 &p_point_A, i
void GodotBodySoftBodyPair3D::validate_contacts() {
// Make sure to erase contacts that are no longer valid.
- const Transform3D &transform_A = body->get_transform();
+ real_t max_separation = space->get_contact_max_separation();
+ real_t max_separation2 = max_separation * max_separation;
- real_t contact_max_separation = space->get_contact_max_separation();
+ const Transform3D &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];
- Vector3 global_A = transform_A.xform(c.local_A);
- Vector3 global_B = soft_body->get_node_position(c.index_B) + c.local_B;
- Vector3 axis = global_A - global_B;
- real_t depth = axis.dot(c.normal);
+ bool erase = false;
+ if (!c.used) {
+ // Was left behind in previous frame.
+ erase = true;
+ } else {
+ c.used = false;
+
+ Vector3 global_A = transform_A.xform(c.local_A);
+ Vector3 global_B = soft_body->get_node_position(c.index_B) + c.local_B;
+ Vector3 axis = global_A - global_B;
+ real_t depth = axis.dot(c.normal);
+
+ if (depth < -max_separation || (global_B + c.normal * depth - global_A).length_squared() > max_separation2) {
+ erase = true;
+ }
+ }
- if (depth < -contact_max_separation || (global_B + c.normal * depth - global_A).length() > contact_max_separation) {
+ if (erase) {
// Contact no longer needed, remove.
if ((contact_index + 1) < contact_count) {
// Swap with the last one.
@@ -640,7 +658,7 @@ bool GodotBodySoftBodyPair3D::pre_solve(real_t p_step) {
real_t max_penetration = space->get_contact_max_allowed_penetration();
- real_t bias = (real_t)0.3;
+ real_t bias = space->get_contact_bias();
GodotShape3D *shape_A_ptr = body->get_shape(body_shape);
@@ -723,8 +741,6 @@ bool GodotBodySoftBodyPair3D::pre_solve(real_t p_step) {
if (soft_body_collides) {
soft_body->apply_node_impulse(c.index_B, j_vec);
}
- c.acc_bias_impulse = 0;
- c.acc_bias_impulse_center_of_mass = 0;
c.bounce = body->get_bounce();
diff --git a/servers/physics_3d/godot_body_pair_3d.h b/servers/physics_3d/godot_body_pair_3d.h
index c0a2424e05..7c2c31704b 100644
--- a/servers/physics_3d/godot_body_pair_3d.h
+++ b/servers/physics_3d/godot_body_pair_3d.h
@@ -54,6 +54,7 @@ protected:
real_t depth = 0.0;
bool active = false;
+ bool used = false;
Vector3 rA, rB; // Offset in world orientation with respect to center of mass
};
diff --git a/servers/physics_3d/godot_physics_server_3d.cpp b/servers/physics_3d/godot_physics_server_3d.cpp
index 9acae62382..573a5d373f 100644
--- a/servers/physics_3d/godot_physics_server_3d.cpp
+++ b/servers/physics_3d/godot_physics_server_3d.cpp
@@ -1574,20 +1574,15 @@ void GodotPhysicsServer3D::free(RID p_rid) {
} else {
ERR_FAIL_MSG("Invalid ID.");
}
-};
+}
void GodotPhysicsServer3D::set_active(bool p_active) {
active = p_active;
-};
-
-void GodotPhysicsServer3D::set_collision_iterations(int p_iterations) {
- iterations = p_iterations;
-};
+}
void GodotPhysicsServer3D::init() {
- iterations = 8; // 8?
stepper = memnew(GodotStep3D);
-};
+}
void GodotPhysicsServer3D::step(real_t p_step) {
#ifndef _3D_DISABLED
@@ -1602,7 +1597,7 @@ void GodotPhysicsServer3D::step(real_t p_step) {
active_objects = 0;
collision_pairs = 0;
for (Set<const GodotSpace3D *>::Element *E = active_spaces.front(); E; E = E->next()) {
- stepper->step((GodotSpace3D *)E->get(), p_step, iterations);
+ stepper->step((GodotSpace3D *)E->get(), p_step);
island_count += E->get()->get_island_count();
active_objects += E->get()->get_active_objects();
collision_pairs += E->get()->get_collision_pairs();
@@ -1612,7 +1607,7 @@ void GodotPhysicsServer3D::step(real_t p_step) {
void GodotPhysicsServer3D::sync() {
doing_sync = true;
-};
+}
void GodotPhysicsServer3D::flush_queries() {
#ifndef _3D_DISABLED
@@ -1665,15 +1660,15 @@ void GodotPhysicsServer3D::flush_queries() {
EngineDebugger::profiler_add_frame_data("servers", values);
}
#endif
-};
+}
void GodotPhysicsServer3D::end_sync() {
doing_sync = false;
-};
+}
void GodotPhysicsServer3D::finish() {
memdelete(stepper);
-};
+}
int GodotPhysicsServer3D::get_process_info(ProcessInfo p_info) {
switch (p_info) {
diff --git a/servers/physics_3d/godot_physics_server_3d.h b/servers/physics_3d/godot_physics_server_3d.h
index f5c8e0f60d..be9bbea76b 100644
--- a/servers/physics_3d/godot_physics_server_3d.h
+++ b/servers/physics_3d/godot_physics_server_3d.h
@@ -44,7 +44,6 @@ class GodotPhysicsServer3D : public PhysicsServer3D {
friend class GodotPhysicsDirectSpaceState3D;
bool active = true;
- int iterations = 0;
int island_count = 0;
int active_objects = 0;
@@ -364,8 +363,6 @@ public:
virtual void end_sync() override;
virtual void finish() override;
- virtual void set_collision_iterations(int p_iterations) override;
-
virtual bool is_flushing_queries() const override { return flushing_queries; }
int get_process_info(ProcessInfo p_info) override;
diff --git a/servers/physics_3d/godot_space_3d.cpp b/servers/physics_3d/godot_space_3d.cpp
index f503273c88..b2a8b00bca 100644
--- a/servers/physics_3d/godot_space_3d.cpp
+++ b/servers/physics_3d/godot_space_3d.cpp
@@ -1173,9 +1173,12 @@ void GodotSpace3D::set_param(PhysicsServer3D::SpaceParameter p_param, real_t p_v
case PhysicsServer3D::SPACE_PARAM_CONTACT_MAX_SEPARATION:
contact_max_separation = p_value;
break;
- case PhysicsServer3D::SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION:
+ case PhysicsServer3D::SPACE_PARAM_CONTACT_MAX_ALLOWED_PENETRATION:
contact_max_allowed_penetration = p_value;
break;
+ case PhysicsServer3D::SPACE_PARAM_CONTACT_DEFAULT_BIAS:
+ contact_bias = p_value;
+ break;
case PhysicsServer3D::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_THRESHOLD:
body_linear_velocity_sleep_threshold = p_value;
break;
@@ -1191,6 +1194,9 @@ void GodotSpace3D::set_param(PhysicsServer3D::SpaceParameter p_param, real_t p_v
case PhysicsServer3D::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS:
constraint_bias = p_value;
break;
+ case PhysicsServer3D::SPACE_PARAM_SOLVER_ITERATIONS:
+ solver_iterations = p_value;
+ break;
}
}
@@ -1200,8 +1206,10 @@ real_t GodotSpace3D::get_param(PhysicsServer3D::SpaceParameter p_param) const {
return contact_recycle_radius;
case PhysicsServer3D::SPACE_PARAM_CONTACT_MAX_SEPARATION:
return contact_max_separation;
- case PhysicsServer3D::SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION:
+ case PhysicsServer3D::SPACE_PARAM_CONTACT_MAX_ALLOWED_PENETRATION:
return contact_max_allowed_penetration;
+ case PhysicsServer3D::SPACE_PARAM_CONTACT_DEFAULT_BIAS:
+ return contact_bias;
case PhysicsServer3D::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_THRESHOLD:
return body_linear_velocity_sleep_threshold;
case PhysicsServer3D::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_THRESHOLD:
@@ -1212,6 +1220,8 @@ real_t GodotSpace3D::get_param(PhysicsServer3D::SpaceParameter p_param) const {
return body_angular_velocity_damp_ratio;
case PhysicsServer3D::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS:
return constraint_bias;
+ case PhysicsServer3D::SPACE_PARAM_SOLVER_ITERATIONS:
+ return solver_iterations;
}
return 0;
}
diff --git a/servers/physics_3d/godot_space_3d.h b/servers/physics_3d/godot_space_3d.h
index aa5e965751..b9aeee7583 100644
--- a/servers/physics_3d/godot_space_3d.h
+++ b/servers/physics_3d/godot_space_3d.h
@@ -93,9 +93,12 @@ private:
GodotArea3D *area = nullptr;
+ int solver_iterations = 16;
+
real_t contact_recycle_radius = 0.01;
real_t contact_max_separation = 0.05;
real_t contact_max_allowed_penetration = 0.01;
+ real_t contact_bias = 0.8;
real_t constraint_bias = 0.01;
enum {
@@ -159,9 +162,11 @@ public:
void remove_object(GodotCollisionObject3D *p_object);
const Set<GodotCollisionObject3D *> &get_objects() const;
+ _FORCE_INLINE_ int get_solver_iterations() const { return solver_iterations; }
_FORCE_INLINE_ real_t get_contact_recycle_radius() const { return contact_recycle_radius; }
_FORCE_INLINE_ real_t get_contact_max_separation() const { return contact_max_separation; }
_FORCE_INLINE_ real_t get_contact_max_allowed_penetration() const { return contact_max_allowed_penetration; }
+ _FORCE_INLINE_ real_t get_contact_bias() const { return contact_bias; }
_FORCE_INLINE_ real_t get_constraint_bias() const { return constraint_bias; }
_FORCE_INLINE_ real_t get_body_linear_velocity_sleep_threshold() const { return body_linear_velocity_sleep_threshold; }
_FORCE_INLINE_ real_t get_body_angular_velocity_sleep_threshold() const { return body_angular_velocity_sleep_threshold; }
diff --git a/servers/physics_3d/godot_step_3d.cpp b/servers/physics_3d/godot_step_3d.cpp
index 5453c4415c..6332532f6e 100644
--- a/servers/physics_3d/godot_step_3d.cpp
+++ b/servers/physics_3d/godot_step_3d.cpp
@@ -181,14 +181,14 @@ void GodotStep3D::_check_suspend(const LocalVector<GodotBody3D *> &p_body_island
}
}
-void GodotStep3D::step(GodotSpace3D *p_space, real_t p_delta, int p_iterations) {
+void GodotStep3D::step(GodotSpace3D *p_space, real_t p_delta) {
p_space->lock(); // can't access space during this
p_space->setup(); //update inertias, etc
p_space->set_last_step(p_delta);
- iterations = p_iterations;
+ iterations = p_space->get_solver_iterations();
delta = p_delta;
const SelfList<GodotBody3D>::List *body_list = &p_space->get_active_body_list();
diff --git a/servers/physics_3d/godot_step_3d.h b/servers/physics_3d/godot_step_3d.h
index 23ede4feff..10389713f6 100644
--- a/servers/physics_3d/godot_step_3d.h
+++ b/servers/physics_3d/godot_step_3d.h
@@ -56,7 +56,7 @@ class GodotStep3D {
void _check_suspend(const LocalVector<GodotBody3D *> &p_body_island) const;
public:
- void step(GodotSpace3D *p_space, real_t p_delta, int p_iterations);
+ void step(GodotSpace3D *p_space, real_t p_delta);
GodotStep3D();
~GodotStep3D();
};
generated by cgit v1.2.3 (git 2.25.1) at 2024-11-07 16:31:59 +0000