summaryrefslogtreecommitdiff
path: root/servers/physics_2d
diff options
context:
space:
mode:
Diffstat (limited to 'servers/physics_2d')
-rw-r--r--servers/physics_2d/body_pair_2d_sw.cpp40
-rw-r--r--servers/physics_2d/body_pair_2d_sw.h4
-rw-r--r--servers/physics_2d/collision_object_2d_sw.h4
-rw-r--r--servers/physics_2d/space_2d_sw.cpp2
4 files changed, 28 insertions, 22 deletions
diff --git a/servers/physics_2d/body_pair_2d_sw.cpp b/servers/physics_2d/body_pair_2d_sw.cpp
index 71d985a6c5..91d747b492 100644
--- a/servers/physics_2d/body_pair_2d_sw.cpp
+++ b/servers/physics_2d/body_pair_2d_sw.cpp
@@ -226,16 +226,16 @@ real_t combine_friction(Body2DSW *A, Body2DSW *B) {
}
bool BodyPair2DSW::setup(real_t p_step) {
- dynamic_A = (A->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC);
- dynamic_B = (B->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC);
-
if (!A->interacts_with(B) || A->has_exception(B->get_self()) || B->has_exception(A->get_self())) {
collided = false;
return false;
}
+ collide_A = (A->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC) && A->collides_with(B);
+ collide_B = (B->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC) && B->collides_with(A);
+
report_contacts_only = false;
- if (!dynamic_A && !dynamic_B) {
+ if (!collide_A && !collide_B) {
if ((A->get_max_contacts_reported() > 0) || (B->get_max_contacts_reported() > 0)) {
report_contacts_only = true;
} else {
@@ -275,13 +275,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 && dynamic_A) {
+ if (A->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_RAY && collide_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 && dynamic_B) {
+ if (B->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_RAY && collide_B) {
if (_test_ccd(p_step, B, shape_B, xform_B, A, shape_A, xform_A, true)) {
collided = true;
}
@@ -374,6 +374,12 @@ bool BodyPair2DSW::pre_solve(real_t p_step) {
const Transform2D &transform_A = A->get_transform();
const Transform2D &transform_B = B->get_transform();
+ real_t inv_inertia_A = collide_A ? A->get_inv_inertia() : 0.0;
+ real_t inv_inertia_B = collide_B ? B->get_inv_inertia() : 0.0;
+
+ real_t inv_mass_A = collide_A ? A->get_inv_mass() : 0.0;
+ real_t inv_mass_B = collide_B ? B->get_inv_mass() : 0.0;
+
for (int i = 0; i < contact_count; i++) {
Contact &c = contacts[i];
c.active = false;
@@ -384,7 +390,7 @@ bool BodyPair2DSW::pre_solve(real_t p_step) {
Vector2 axis = global_A - global_B;
real_t depth = axis.dot(c.normal);
- if (depth <= 0 || !c.reused) {
+ if (depth <= 0.0 || !c.reused) {
continue;
}
@@ -416,15 +422,15 @@ bool BodyPair2DSW::pre_solve(real_t p_step) {
// Precompute normal mass, tangent mass, and bias.
real_t rnA = c.rA.dot(c.normal);
real_t rnB = c.rB.dot(c.normal);
- real_t kNormal = A->get_inv_mass() + B->get_inv_mass();
- kNormal += A->get_inv_inertia() * (c.rA.dot(c.rA) - rnA * rnA) + B->get_inv_inertia() * (c.rB.dot(c.rB) - rnB * rnB);
+ real_t kNormal = inv_mass_A + inv_mass_B;
+ kNormal += inv_inertia_A * (c.rA.dot(c.rA) - rnA * rnA) + inv_inertia_B * (c.rB.dot(c.rB) - rnB * rnB);
c.mass_normal = 1.0f / kNormal;
Vector2 tangent = c.normal.orthogonal();
real_t rtA = c.rA.dot(tangent);
real_t rtB = c.rB.dot(tangent);
- real_t kTangent = A->get_inv_mass() + B->get_inv_mass();
- kTangent += A->get_inv_inertia() * (c.rA.dot(c.rA) - rtA * rtA) + B->get_inv_inertia() * (c.rB.dot(c.rB) - rtB * rtB);
+ real_t kTangent = inv_mass_A + inv_mass_B;
+ kTangent += inv_inertia_A * (c.rA.dot(c.rA) - rtA * rtA) + inv_inertia_B * (c.rB.dot(c.rB) - rtB * rtB);
c.mass_tangent = 1.0f / kTangent;
c.bias = -bias * inv_dt * MIN(0.0f, -depth + max_penetration);
@@ -436,10 +442,10 @@ bool BodyPair2DSW::pre_solve(real_t p_step) {
// Apply normal + friction impulse
Vector2 P = c.acc_normal_impulse * c.normal + c.acc_tangent_impulse * tangent;
- if (dynamic_A) {
+ if (collide_A) {
A->apply_impulse(-P, c.rA);
}
- if (dynamic_B) {
+ if (collide_B) {
B->apply_impulse(P, c.rB);
}
}
@@ -493,10 +499,10 @@ void BodyPair2DSW::solve(real_t p_step) {
Vector2 jb = c.normal * (c.acc_bias_impulse - jbnOld);
- if (dynamic_A) {
+ if (collide_A) {
A->apply_bias_impulse(-jb, c.rA);
}
- if (dynamic_B) {
+ if (collide_B) {
B->apply_bias_impulse(jb, c.rB);
}
@@ -513,10 +519,10 @@ void BodyPair2DSW::solve(real_t p_step) {
Vector2 j = c.normal * (c.acc_normal_impulse - jnOld) + tangent * (c.acc_tangent_impulse - jtOld);
- if (dynamic_A) {
+ if (collide_A) {
A->apply_impulse(-j, c.rA);
}
- if (dynamic_B) {
+ if (collide_B) {
B->apply_impulse(j, c.rB);
}
}
diff --git a/servers/physics_2d/body_pair_2d_sw.h b/servers/physics_2d/body_pair_2d_sw.h
index 4b42b44c92..849a7e2430 100644
--- a/servers/physics_2d/body_pair_2d_sw.h
+++ b/servers/physics_2d/body_pair_2d_sw.h
@@ -50,8 +50,8 @@ class BodyPair2DSW : public Constraint2DSW {
int shape_A = 0;
int shape_B = 0;
- bool dynamic_A = false;
- bool dynamic_B = false;
+ bool collide_A = false;
+ bool collide_B = false;
Space2DSW *space = nullptr;
diff --git a/servers/physics_2d/collision_object_2d_sw.h b/servers/physics_2d/collision_object_2d_sw.h
index 14adb0bb18..55ffa9b1b8 100644
--- a/servers/physics_2d/collision_object_2d_sw.h
+++ b/servers/physics_2d/collision_object_2d_sw.h
@@ -186,8 +186,8 @@ public:
void set_pickable(bool p_pickable) { pickable = p_pickable; }
_FORCE_INLINE_ bool is_pickable() const { return pickable; }
- _FORCE_INLINE_ bool layer_in_mask(CollisionObject2DSW *p_other) const {
- return collision_layer & p_other->collision_mask;
+ _FORCE_INLINE_ bool collides_with(CollisionObject2DSW *p_other) const {
+ return p_other->collision_layer & collision_mask;
}
_FORCE_INLINE_ bool interacts_with(CollisionObject2DSW *p_other) const {
diff --git a/servers/physics_2d/space_2d_sw.cpp b/servers/physics_2d/space_2d_sw.cpp
index 43329fed2f..590c93a7be 100644
--- a/servers/physics_2d/space_2d_sw.cpp
+++ b/servers/physics_2d/space_2d_sw.cpp
@@ -508,7 +508,7 @@ int Space2DSW::_cull_aabb_for_body(Body2DSW *p_body, const Rect2 &p_aabb) {
keep = false;
} else if (intersection_query_results[i]->get_type() == CollisionObject2DSW::TYPE_AREA) {
keep = false;
- } else if (!p_body->layer_in_mask(static_cast<Body2DSW *>(intersection_query_results[i]))) {
+ } else if (!p_body->collides_with(static_cast<Body2DSW *>(intersection_query_results[i]))) {
keep = false;
} else if (static_cast<Body2DSW *>(intersection_query_results[i])->has_exception(p_body->get_self()) || p_body->has_exception(intersection_query_results[i]->get_self())) {
keep = false;
generated by cgit v1.2.3 (git 2.25.1) at 2024-11-27 06:38:40 +0000