summaryrefslogtreecommitdiff
path: root/servers/physics_2d
diff options
context:
space:
mode:
Diffstat (limited to 'servers/physics_2d')
-rw-r--r--servers/physics_2d/area_pair_2d_sw.cpp4
-rw-r--r--servers/physics_2d/body_2d_sw.cpp6
-rw-r--r--servers/physics_2d/body_pair_2d_sw.cpp42
-rw-r--r--servers/physics_2d/body_pair_2d_sw.h4
-rw-r--r--servers/physics_2d/collision_object_2d_sw.h6
-rw-r--r--servers/physics_2d/space_2d_sw.cpp4
-rw-r--r--servers/physics_2d/step_2d_sw.cpp6
7 files changed, 41 insertions, 31 deletions
diff --git a/servers/physics_2d/area_pair_2d_sw.cpp b/servers/physics_2d/area_pair_2d_sw.cpp
index 5cc5bae09b..5ca16cb6fc 100644
--- a/servers/physics_2d/area_pair_2d_sw.cpp
+++ b/servers/physics_2d/area_pair_2d_sw.cpp
@@ -33,7 +33,7 @@
bool AreaPair2DSW::setup(real_t p_step) {
bool result = false;
- if (area->test_collision_mask(body) && CollisionSolver2DSW::solve(body->get_shape(body_shape), body->get_transform() * body->get_shape_transform(body_shape), Vector2(), area->get_shape(area_shape), area->get_transform() * area->get_shape_transform(area_shape), Vector2(), nullptr, this)) {
+ if (area->interacts_with(body) && CollisionSolver2DSW::solve(body->get_shape(body_shape), body->get_transform() * body->get_shape_transform(body_shape), Vector2(), area->get_shape(area_shape), area->get_transform() * area->get_shape_transform(area_shape), Vector2(), nullptr, this)) {
result = true;
}
@@ -110,7 +110,7 @@ AreaPair2DSW::~AreaPair2DSW() {
bool Area2Pair2DSW::setup(real_t p_step) {
bool result = false;
- if (area_a->test_collision_mask(area_b) && CollisionSolver2DSW::solve(area_a->get_shape(shape_a), area_a->get_transform() * area_a->get_shape_transform(shape_a), Vector2(), area_b->get_shape(shape_b), area_b->get_transform() * area_b->get_shape_transform(shape_b), Vector2(), nullptr, this)) {
+ if (area_a->interacts_with(area_b) && CollisionSolver2DSW::solve(area_a->get_shape(shape_a), area_a->get_transform() * area_a->get_shape_transform(shape_a), Vector2(), area_b->get_shape(shape_b), area_b->get_transform() * area_b->get_shape_transform(shape_b), Vector2(), nullptr, this)) {
result = true;
}
diff --git a/servers/physics_2d/body_2d_sw.cpp b/servers/physics_2d/body_2d_sw.cpp
index 6f244deb1e..7aa2f9b7de 100644
--- a/servers/physics_2d/body_2d_sw.cpp
+++ b/servers/physics_2d/body_2d_sw.cpp
@@ -532,13 +532,13 @@ void Body2DSW::integrate_velocities(real_t p_step) {
}
void Body2DSW::wakeup_neighbours() {
- for (List<Pair<Constraint2DSW *, int>>::Element *E = constraint_list.front(); E; E = E->next()) {
- const Constraint2DSW *c = E->get().first;
+ for (const Pair<Constraint2DSW *, int> &E : constraint_list) {
+ const Constraint2DSW *c = E.first;
Body2DSW **n = c->get_body_ptr();
int bc = c->get_body_count();
for (int i = 0; i < bc; i++) {
- if (i == E->get().second) {
+ if (i == E.second) {
continue;
}
Body2DSW *b = n[i];
diff --git a/servers/physics_2d/body_pair_2d_sw.cpp b/servers/physics_2d/body_pair_2d_sw.cpp
index e5eb374fa3..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->test_collision_mask(B) || A->has_exception(B->get_self()) || B->has_exception(A->get_self())) {
+ 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 67da758d14..55ffa9b1b8 100644
--- a/servers/physics_2d/collision_object_2d_sw.h
+++ b/servers/physics_2d/collision_object_2d_sw.h
@@ -186,7 +186,11 @@ public:
void set_pickable(bool p_pickable) { pickable = p_pickable; }
_FORCE_INLINE_ bool is_pickable() const { return pickable; }
- _FORCE_INLINE_ bool test_collision_mask(CollisionObject2DSW *p_other) const {
+ _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 {
return collision_layer & p_other->collision_mask || p_other->collision_layer & collision_mask;
}
diff --git a/servers/physics_2d/space_2d_sw.cpp b/servers/physics_2d/space_2d_sw.cpp
index 82da5774db..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 ((static_cast<Body2DSW *>(intersection_query_results[i])->test_collision_mask(p_body)) == 0) {
+ } 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;
@@ -1188,7 +1188,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
}
void *Space2DSW::_broadphase_pair(CollisionObject2DSW *A, int p_subindex_A, CollisionObject2DSW *B, int p_subindex_B, void *p_self) {
- if (!A->test_collision_mask(B)) {
+ if (!A->interacts_with(B)) {
return nullptr;
}
diff --git a/servers/physics_2d/step_2d_sw.cpp b/servers/physics_2d/step_2d_sw.cpp
index b8cb4cddc5..8b30160cc1 100644
--- a/servers/physics_2d/step_2d_sw.cpp
+++ b/servers/physics_2d/step_2d_sw.cpp
@@ -46,8 +46,8 @@ void Step2DSW::_populate_island(Body2DSW *p_body, LocalVector<Body2DSW *> &p_bod
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;
+ for (const Pair<Constraint2DSW *, int> &E : p_body->get_constraint_list()) {
+ Constraint2DSW *constraint = (Constraint2DSW *)E.first;
if (constraint->get_island_step() == _step) {
continue; // Already processed.
}
@@ -56,7 +56,7 @@ void Step2DSW::_populate_island(Body2DSW *p_body, LocalVector<Body2DSW *> &p_bod
all_constraints.push_back(constraint);
for (int i = 0; i < constraint->get_body_count(); i++) {
- if (i == E->get().second) {
+ if (i == E.second) {
continue;
}
Body2DSW *other_body = constraint->get_body_ptr()[i];
generated by cgit v1.2.3 (git 2.25.1) at 2024-11-19 02:18:05 +0000