summaryrefslogtreecommitdiff
path: root/servers/physics_3d
diff options
context:
space:
mode:
Diffstat (limited to 'servers/physics_3d')
-rw-r--r--servers/physics_3d/area_3d_sw.cpp27
-rw-r--r--servers/physics_3d/body_3d_sw.h40
-rw-r--r--servers/physics_3d/body_pair_3d_sw.cpp12
-rw-r--r--servers/physics_3d/collision_object_3d_sw.h10
-rw-r--r--servers/physics_3d/joints/cone_twist_joint_3d_sw.cpp4
-rw-r--r--servers/physics_3d/joints/generic_6dof_joint_3d_sw.cpp4
-rw-r--r--servers/physics_3d/joints/hinge_joint_3d_sw.cpp4
-rw-r--r--servers/physics_3d/joints/pin_joint_3d_sw.cpp4
-rw-r--r--servers/physics_3d/joints/slider_joint_3d_sw.cpp8
-rw-r--r--servers/physics_3d/physics_server_3d_sw.cpp8
-rw-r--r--servers/physics_3d/physics_server_3d_sw.h4
11 files changed, 70 insertions, 55 deletions
diff --git a/servers/physics_3d/area_3d_sw.cpp b/servers/physics_3d/area_3d_sw.cpp
index 98237dd91c..571f1435de 100644
--- a/servers/physics_3d/area_3d_sw.cpp
+++ b/servers/physics_3d/area_3d_sw.cpp
@@ -213,9 +213,10 @@ void Area3DSW::call_queries() {
return;
}
- for (Map<BodyKey, BodyState>::Element *E = monitored_bodies.front(); E; E = E->next()) {
- if (E->get().state == 0) {
- continue; //nothing happened
+ for (Map<BodyKey, BodyState>::Element *E = monitored_bodies.front(); E;) {
+ if (E->get().state == 0) { // Nothing happened
+ E = E->next();
+ continue;
}
res[0] = E->get().state > 0 ? PhysicsServer3D::AREA_BODY_ADDED : PhysicsServer3D::AREA_BODY_REMOVED;
@@ -224,13 +225,15 @@ void Area3DSW::call_queries() {
res[3] = E->key().body_shape;
res[4] = E->key().area_shape;
+ Map<BodyKey, BodyState>::Element *next = E->next();
+ monitored_bodies.erase(E);
+ E = next;
+
Callable::CallError ce;
obj->call(monitor_callback_method, (const Variant **)resptr, 5, ce);
}
}
- monitored_bodies.clear();
-
if (area_monitor_callback_id.is_valid() && !monitored_areas.empty()) {
Variant res[5];
Variant *resptr[5];
@@ -245,9 +248,10 @@ void Area3DSW::call_queries() {
return;
}
- for (Map<BodyKey, BodyState>::Element *E = monitored_areas.front(); E; E = E->next()) {
- if (E->get().state == 0) {
- continue; //nothing happened
+ for (Map<BodyKey, BodyState>::Element *E = monitored_areas.front(); E;) {
+ if (E->get().state == 0) { // Nothing happened
+ E = E->next();
+ continue;
}
res[0] = E->get().state > 0 ? PhysicsServer3D::AREA_BODY_ADDED : PhysicsServer3D::AREA_BODY_REMOVED;
@@ -256,13 +260,14 @@ void Area3DSW::call_queries() {
res[3] = E->key().body_shape;
res[4] = E->key().area_shape;
+ Map<BodyKey, BodyState>::Element *next = E->next();
+ monitored_areas.erase(E);
+ E = next;
+
Callable::CallError ce;
obj->call(area_monitor_callback_method, (const Variant **)resptr, 5, ce);
}
}
-
- monitored_areas.clear();
- //get_space()->area_remove_from_monitor_query_list(&monitor_query_list);
}
Area3DSW::Area3DSW() :
diff --git a/servers/physics_3d/body_3d_sw.h b/servers/physics_3d/body_3d_sw.h
index 483ea58620..2878c97c9d 100644
--- a/servers/physics_3d/body_3d_sw.h
+++ b/servers/physics_3d/body_3d_sw.h
@@ -216,23 +216,23 @@ public:
_FORCE_INLINE_ const Vector3 &get_biased_linear_velocity() const { return biased_linear_velocity; }
_FORCE_INLINE_ const Vector3 &get_biased_angular_velocity() const { return biased_angular_velocity; }
- _FORCE_INLINE_ void apply_central_impulse(const Vector3 &p_j) {
- linear_velocity += p_j * _inv_mass;
+ _FORCE_INLINE_ void apply_central_impulse(const Vector3 &p_impulse) {
+ linear_velocity += p_impulse * _inv_mass;
}
- _FORCE_INLINE_ void apply_impulse(const Vector3 &p_pos, const Vector3 &p_j) {
- linear_velocity += p_j * _inv_mass;
- angular_velocity += _inv_inertia_tensor.xform((p_pos - center_of_mass).cross(p_j));
+ _FORCE_INLINE_ void apply_impulse(const Vector3 &p_impulse, const Vector3 &p_position = Vector3()) {
+ linear_velocity += p_impulse * _inv_mass;
+ angular_velocity += _inv_inertia_tensor.xform((p_position - center_of_mass).cross(p_impulse));
}
- _FORCE_INLINE_ void apply_torque_impulse(const Vector3 &p_j) {
- angular_velocity += _inv_inertia_tensor.xform(p_j);
+ _FORCE_INLINE_ void apply_torque_impulse(const Vector3 &p_impulse) {
+ angular_velocity += _inv_inertia_tensor.xform(p_impulse);
}
- _FORCE_INLINE_ void apply_bias_impulse(const Vector3 &p_pos, const Vector3 &p_j, real_t p_max_delta_av = -1.0) {
- biased_linear_velocity += p_j * _inv_mass;
+ _FORCE_INLINE_ void apply_bias_impulse(const Vector3 &p_impulse, const Vector3 &p_position = Vector3(), real_t p_max_delta_av = -1.0) {
+ biased_linear_velocity += p_impulse * _inv_mass;
if (p_max_delta_av != 0.0) {
- Vector3 delta_av = _inv_inertia_tensor.xform((p_pos - center_of_mass).cross(p_j));
+ Vector3 delta_av = _inv_inertia_tensor.xform((p_position - center_of_mass).cross(p_impulse));
if (p_max_delta_av > 0 && delta_av.length() > p_max_delta_av) {
delta_av = delta_av.normalized() * p_max_delta_av;
}
@@ -240,17 +240,17 @@ public:
}
}
- _FORCE_INLINE_ void apply_bias_torque_impulse(const Vector3 &p_j) {
- biased_angular_velocity += _inv_inertia_tensor.xform(p_j);
+ _FORCE_INLINE_ void apply_bias_torque_impulse(const Vector3 &p_impulse) {
+ biased_angular_velocity += _inv_inertia_tensor.xform(p_impulse);
}
_FORCE_INLINE_ void add_central_force(const Vector3 &p_force) {
applied_force += p_force;
}
- _FORCE_INLINE_ void add_force(const Vector3 &p_force, const Vector3 &p_pos) {
+ _FORCE_INLINE_ void add_force(const Vector3 &p_force, const Vector3 &p_position = Vector3()) {
applied_force += p_force;
- applied_torque += (p_pos - center_of_mass).cross(p_force);
+ applied_torque += (p_position - center_of_mass).cross(p_force);
}
_FORCE_INLINE_ void add_torque(const Vector3 &p_torque) {
@@ -403,11 +403,15 @@ public:
virtual Transform get_transform() const { return body->get_transform(); }
virtual void add_central_force(const Vector3 &p_force) { body->add_central_force(p_force); }
- virtual void add_force(const Vector3 &p_force, const Vector3 &p_pos) { body->add_force(p_force, p_pos); }
+ virtual void add_force(const Vector3 &p_force, const Vector3 &p_position = Vector3()) {
+ body->add_force(p_force, p_position);
+ }
virtual void add_torque(const Vector3 &p_torque) { body->add_torque(p_torque); }
- virtual void apply_central_impulse(const Vector3 &p_j) { body->apply_central_impulse(p_j); }
- virtual void apply_impulse(const Vector3 &p_pos, const Vector3 &p_j) { body->apply_impulse(p_pos, p_j); }
- virtual void apply_torque_impulse(const Vector3 &p_j) { body->apply_torque_impulse(p_j); }
+ virtual void apply_central_impulse(const Vector3 &p_impulse) { body->apply_central_impulse(p_impulse); }
+ virtual void apply_impulse(const Vector3 &p_impulse, const Vector3 &p_position = Vector3()) {
+ body->apply_impulse(p_impulse, p_position);
+ }
+ virtual void apply_torque_impulse(const Vector3 &p_impulse) { body->apply_torque_impulse(p_impulse); }
virtual void set_sleep_state(bool p_sleep) { body->set_active(!p_sleep); }
virtual bool is_sleeping() const { return !body->is_active(); }
diff --git a/servers/physics_3d/body_pair_3d_sw.cpp b/servers/physics_3d/body_pair_3d_sw.cpp
index a4f86badbe..848138940e 100644
--- a/servers/physics_3d/body_pair_3d_sw.cpp
+++ b/servers/physics_3d/body_pair_3d_sw.cpp
@@ -321,8 +321,8 @@ 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(c.rA + A->get_center_of_mass(), -j_vec);
- B->apply_impulse(c.rB + B->get_center_of_mass(), j_vec);
+ A->apply_impulse(-j_vec, c.rA + A->get_center_of_mass());
+ 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;
@@ -404,8 +404,8 @@ void BodyPair3DSW::solve(real_t p_step) {
Vector3 j = c.normal * (c.acc_normal_impulse - jnOld);
- A->apply_impulse(c.rA + A->get_center_of_mass(), -j);
- B->apply_impulse(c.rB + B->get_center_of_mass(), j);
+ A->apply_impulse(-j, c.rA + A->get_center_of_mass());
+ B->apply_impulse(j, c.rB + B->get_center_of_mass());
c.active = true;
}
@@ -447,8 +447,8 @@ void BodyPair3DSW::solve(real_t p_step) {
jt = c.acc_tangent_impulse - jtOld;
- A->apply_impulse(c.rA + A->get_center_of_mass(), -jt);
- B->apply_impulse(c.rB + B->get_center_of_mass(), jt);
+ A->apply_impulse(-jt, c.rA + A->get_center_of_mass());
+ B->apply_impulse(jt, c.rB + B->get_center_of_mass());
c.active = true;
}
diff --git a/servers/physics_3d/collision_object_3d_sw.h b/servers/physics_3d/collision_object_3d_sw.h
index 9506f14402..a3a5787ced 100644
--- a/servers/physics_3d/collision_object_3d_sw.h
+++ b/servers/physics_3d/collision_object_3d_sw.h
@@ -142,10 +142,16 @@ public:
return shapes[p_idx].disabled;
}
- _FORCE_INLINE_ void set_collision_layer(uint32_t p_layer) { collision_layer = p_layer; }
+ _FORCE_INLINE_ void set_collision_layer(uint32_t p_layer) {
+ collision_layer = p_layer;
+ _shape_changed();
+ }
_FORCE_INLINE_ uint32_t get_collision_layer() const { return collision_layer; }
- _FORCE_INLINE_ void set_collision_mask(uint32_t p_mask) { collision_mask = p_mask; }
+ _FORCE_INLINE_ void set_collision_mask(uint32_t p_mask) {
+ collision_mask = p_mask;
+ _shape_changed();
+ }
_FORCE_INLINE_ uint32_t get_collision_mask() const { return collision_mask; }
_FORCE_INLINE_ bool test_collision_mask(CollisionObject3DSW *p_other) const {
diff --git a/servers/physics_3d/joints/cone_twist_joint_3d_sw.cpp b/servers/physics_3d/joints/cone_twist_joint_3d_sw.cpp
index 9d10ede608..789d6687a4 100644
--- a/servers/physics_3d/joints/cone_twist_joint_3d_sw.cpp
+++ b/servers/physics_3d/joints/cone_twist_joint_3d_sw.cpp
@@ -261,8 +261,8 @@ 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(pivotAInW - A->get_transform().origin, impulse_vector);
- B->apply_impulse(pivotBInW - B->get_transform().origin, -impulse_vector);
+ A->apply_impulse(impulse_vector, pivotAInW - A->get_transform().origin);
+ B->apply_impulse(-impulse_vector, pivotBInW - B->get_transform().origin);
}
}
diff --git a/servers/physics_3d/joints/generic_6dof_joint_3d_sw.cpp b/servers/physics_3d/joints/generic_6dof_joint_3d_sw.cpp
index 423bbc0dfd..fede40ca65 100644
--- a/servers/physics_3d/joints/generic_6dof_joint_3d_sw.cpp
+++ b/servers/physics_3d/joints/generic_6dof_joint_3d_sw.cpp
@@ -205,8 +205,8 @@ real_t G6DOFTranslationalLimitMotor3DSW::solveLinearAxis(
normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse;
Vector3 impulse_vector = axis_normal_on_a * normalImpulse;
- body1->apply_impulse(rel_pos1, impulse_vector);
- body2->apply_impulse(rel_pos2, -impulse_vector);
+ body1->apply_impulse(impulse_vector, rel_pos1);
+ body2->apply_impulse(-impulse_vector, rel_pos2);
return normalImpulse;
}
diff --git a/servers/physics_3d/joints/hinge_joint_3d_sw.cpp b/servers/physics_3d/joints/hinge_joint_3d_sw.cpp
index a879b4ca7f..52c7389e1f 100644
--- a/servers/physics_3d/joints/hinge_joint_3d_sw.cpp
+++ b/servers/physics_3d/joints/hinge_joint_3d_sw.cpp
@@ -275,8 +275,8 @@ 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(pivotAInW - A->get_transform().origin, impulse_vector);
- B->apply_impulse(pivotBInW - B->get_transform().origin, -impulse_vector);
+ A->apply_impulse(impulse_vector, pivotAInW - A->get_transform().origin);
+ B->apply_impulse(-impulse_vector, pivotBInW - B->get_transform().origin);
}
}
diff --git a/servers/physics_3d/joints/pin_joint_3d_sw.cpp b/servers/physics_3d/joints/pin_joint_3d_sw.cpp
index 230904408b..f028ad88f9 100644
--- a/servers/physics_3d/joints/pin_joint_3d_sw.cpp
+++ b/servers/physics_3d/joints/pin_joint_3d_sw.cpp
@@ -119,8 +119,8 @@ void PinJoint3DSW::solve(real_t p_step) {
m_appliedImpulse += impulse;
Vector3 impulse_vector = normal * impulse;
- A->apply_impulse(pivotAInW - A->get_transform().origin, impulse_vector);
- B->apply_impulse(pivotBInW - B->get_transform().origin, -impulse_vector);
+ A->apply_impulse(impulse_vector, pivotAInW - A->get_transform().origin);
+ B->apply_impulse(-impulse_vector, pivotBInW - B->get_transform().origin);
normal[i] = 0;
}
diff --git a/servers/physics_3d/joints/slider_joint_3d_sw.cpp b/servers/physics_3d/joints/slider_joint_3d_sw.cpp
index 5b4609f24e..43bd49b4b5 100644
--- a/servers/physics_3d/joints/slider_joint_3d_sw.cpp
+++ b/servers/physics_3d/joints/slider_joint_3d_sw.cpp
@@ -197,8 +197,8 @@ 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(m_relPosA, impulse_vector);
- B->apply_impulse(m_relPosB, -impulse_vector);
+ A->apply_impulse(impulse_vector, m_relPosA);
+ B->apply_impulse(-impulse_vector, m_relPosB);
if (m_poweredLinMotor && (!i)) { // apply linear motor
if (m_accumulatedLinMotorImpulse < m_maxLinMotorForce) {
real_t desiredMotorVel = m_targetLinMotorVelocity;
@@ -218,8 +218,8 @@ void SliderJoint3DSW::solve(real_t p_step) {
m_accumulatedLinMotorImpulse = new_acc;
// apply clamped impulse
impulse_vector = normal * normalImpulse;
- A->apply_impulse(m_relPosA, impulse_vector);
- B->apply_impulse(m_relPosB, -impulse_vector);
+ A->apply_impulse(impulse_vector, m_relPosA);
+ B->apply_impulse(-impulse_vector, m_relPosB);
}
}
}
diff --git a/servers/physics_3d/physics_server_3d_sw.cpp b/servers/physics_3d/physics_server_3d_sw.cpp
index 1c2329f2dc..143cc9ebbd 100644
--- a/servers/physics_3d/physics_server_3d_sw.cpp
+++ b/servers/physics_3d/physics_server_3d_sw.cpp
@@ -710,11 +710,11 @@ void PhysicsServer3DSW::body_add_central_force(RID p_body, const Vector3 &p_forc
body->wakeup();
}
-void PhysicsServer3DSW::body_add_force(RID p_body, const Vector3 &p_force, const Vector3 &p_pos) {
+void PhysicsServer3DSW::body_add_force(RID p_body, const Vector3 &p_force, const Vector3 &p_position) {
Body3DSW *body = body_owner.getornull(p_body);
ERR_FAIL_COND(!body);
- body->add_force(p_force, p_pos);
+ body->add_force(p_force, p_position);
body->wakeup();
};
@@ -736,13 +736,13 @@ void PhysicsServer3DSW::body_apply_central_impulse(RID p_body, const Vector3 &p_
body->wakeup();
}
-void PhysicsServer3DSW::body_apply_impulse(RID p_body, const Vector3 &p_pos, const Vector3 &p_impulse) {
+void PhysicsServer3DSW::body_apply_impulse(RID p_body, const Vector3 &p_impulse, const Vector3 &p_position) {
Body3DSW *body = body_owner.getornull(p_body);
ERR_FAIL_COND(!body);
_update_shapes();
- body->apply_impulse(p_pos, p_impulse);
+ body->apply_impulse(p_impulse, p_position);
body->wakeup();
};
diff --git a/servers/physics_3d/physics_server_3d_sw.h b/servers/physics_3d/physics_server_3d_sw.h
index 26230ef674..dccacb063f 100644
--- a/servers/physics_3d/physics_server_3d_sw.h
+++ b/servers/physics_3d/physics_server_3d_sw.h
@@ -206,11 +206,11 @@ public:
virtual Vector3 body_get_applied_torque(RID p_body) const;
virtual void body_add_central_force(RID p_body, const Vector3 &p_force);
- virtual void body_add_force(RID p_body, const Vector3 &p_force, const Vector3 &p_pos);
+ virtual void body_add_force(RID p_body, const Vector3 &p_force, const Vector3 &p_position = Vector3());
virtual void body_add_torque(RID p_body, const Vector3 &p_torque);
virtual void body_apply_central_impulse(RID p_body, const Vector3 &p_impulse);
- virtual void body_apply_impulse(RID p_body, const Vector3 &p_pos, const Vector3 &p_impulse);
+ virtual void body_apply_impulse(RID p_body, const Vector3 &p_impulse, const Vector3 &p_position = Vector3());
virtual void body_apply_torque_impulse(RID p_body, const Vector3 &p_impulse);
virtual void body_set_axis_velocity(RID p_body, const Vector3 &p_axis_velocity);
generated by cgit v1.2.3 (git 2.25.1) at 2025-03-09 22:22:16 +0000