summaryrefslogtreecommitdiff
path: root/servers/physics_2d/joints_2d_sw.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'servers/physics_2d/joints_2d_sw.cpp')
-rw-r--r--servers/physics_2d/joints_2d_sw.cpp122
1 files changed, 80 insertions, 42 deletions
diff --git a/servers/physics_2d/joints_2d_sw.cpp b/servers/physics_2d/joints_2d_sw.cpp
index f503868ba5..5a0a628fbc 100644
--- a/servers/physics_2d/joints_2d_sw.cpp
+++ b/servers/physics_2d/joints_2d_sw.cpp
@@ -97,8 +97,16 @@ normal_relative_velocity(Body2DSW *a, Body2DSW *b, Vector2 rA, Vector2 rB, Vecto
}
bool PinJoint2DSW::setup(real_t p_step) {
+ 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);
+
rA = A->get_transform().basis_xform(anchor_A);
rB = B ? B->get_transform().basis_xform(anchor_B) : anchor_B;
@@ -143,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;
}
@@ -156,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());
@@ -169,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);
}
@@ -205,15 +221,6 @@ PinJoint2DSW::PinJoint2DSW(const Vector2 &p_pos, Body2DSW *p_body_a, Body2DSW *p
}
}
-PinJoint2DSW::~PinJoint2DSW() {
- if (A) {
- A->remove_constraint(this, 0);
- }
- if (B) {
- B->remove_constraint(this, 1);
- }
-}
-
//////////////////////////////////////////////
//////////////////////////////////////////////
//////////////////////////////////////////////
@@ -266,10 +273,19 @@ mult_k(const Vector2 &vr, const Vector2 &k1, const Vector2 &k2) {
}
bool GrooveJoint2DSW::setup(real_t p_step) {
+ 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();
@@ -306,16 +322,24 @@ bool GrooveJoint2DSW::setup(real_t p_step) {
Vector2 delta = (B->get_transform().get_origin() + rB) - (A->get_transform().get_origin() + rA);
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);
+ gbias = (delta * -(_b == 0 ? space->get_constraint_bias() : _b) * (1.0 / p_step)).limit_length(get_max_bias());
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);
@@ -324,12 +348,16 @@ void GrooveJoint2DSW::solve(real_t p_step) {
Vector2 jOld = jn_acc;
j += jOld;
- jn_acc = (((clamp * j.cross(xf_normal)) > 0) ? j : j.project(xf_normal)).clamped(jn_max);
+ jn_acc = (((clamp * j.cross(xf_normal)) > 0) ? j : j.project(xf_normal)).limit_length(jn_max);
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) :
@@ -346,16 +374,18 @@ GrooveJoint2DSW::GrooveJoint2DSW(const Vector2 &p_a_groove1, const Vector2 &p_a_
B->add_constraint(this, 1);
}
-GrooveJoint2DSW::~GrooveJoint2DSW() {
- A->remove_constraint(this, 0);
- B->remove_constraint(this, 1);
-}
-
//////////////////////////////////////////////
//////////////////////////////////////////////
//////////////////////////////////////////////
bool DampedSpringJoint2DSW::setup(real_t p_step) {
+ 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;
+ }
+
rA = A->get_transform().basis_xform(anchor_A);
rB = B->get_transform().basis_xform(anchor_B);
@@ -374,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;
}
@@ -394,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) {
@@ -442,8 +485,3 @@ DampedSpringJoint2DSW::DampedSpringJoint2DSW(const Vector2 &p_anchor_a, const Ve
A->add_constraint(this, 0);
B->add_constraint(this, 1);
}
-
-DampedSpringJoint2DSW::~DampedSpringJoint2DSW() {
- A->remove_constraint(this, 0);
- B->remove_constraint(this, 1);
-}
generated by cgit v1.2.3 (git 2.25.1) at 2025-01-08 14:49:47 +0000