1 files changed, 10 insertions, 11 deletions

diff --git a/servers/physics_2d/joints_2d_sw.cpp b/servers/physics_2d/joints_2d_sw.cpp
index 5a0a628fbc..b46397b8e6 100644
--- a/servers/physics_2d/joints_2d_sw.cpp
+++ b/servers/physics_2d/joints_2d_sw.cpp
@@ -64,17 +64,17 @@ void Joint2DSW::copy_settings_from(Joint2DSW *p_joint) {
 }
 
 static inline real_t k_scalar(Body2DSW *a, Body2DSW *b, const Vector2 &rA, const Vector2 &rB, const Vector2 &n) {
-	real_t value = 0;
+	real_t value = 0.0;
 
 	{
 		value += a->get_inv_mass();
-		real_t rcn = rA.cross(n);
+		real_t rcn = (rA - a->get_center_of_mass()).cross(n);
 		value += a->get_inv_inertia() * rcn * rcn;
 	}
 
 	if (b) {
 		value += b->get_inv_mass();
-		real_t rcn = rB.cross(n);
+		real_t rcn = (rB - b->get_center_of_mass()).cross(n);
 		value += b->get_inv_inertia() * rcn * rcn;
 	}
 
@@ -83,9 +83,9 @@ static inline real_t k_scalar(Body2DSW *a, Body2DSW *b, const Vector2 &rA, const
 
 static inline Vector2
 relative_velocity(Body2DSW *a, Body2DSW *b, Vector2 rA, Vector2 rB) {
-	Vector2 sum = a->get_linear_velocity() - rA.orthogonal() * a->get_angular_velocity();
+	Vector2 sum = a->get_linear_velocity() - (rA - a->get_center_of_mass()).orthogonal() * a->get_angular_velocity();
 	if (b) {
-		return (b->get_linear_velocity() - rB.orthogonal() * b->get_angular_velocity()) - sum;
+		return (b->get_linear_velocity() - (rB - b->get_center_of_mass()).orthogonal() * b->get_angular_velocity()) - sum;
 	} else {
 		return -sum;
 	}
@@ -172,11 +172,11 @@ bool PinJoint2DSW::pre_solve(real_t p_step) {
 
 void PinJoint2DSW::solve(real_t p_step) {
 	// compute relative velocity
-	Vector2 vA = A->get_linear_velocity() - custom_cross(rA, A->get_angular_velocity());
+	Vector2 vA = A->get_linear_velocity() - custom_cross(rA - A->get_center_of_mass(), A->get_angular_velocity());
 
 	Vector2 rel_vel;
 	if (B) {
-		rel_vel = B->get_linear_velocity() - custom_cross(rB, B->get_angular_velocity()) - vA;
+		rel_vel = B->get_linear_velocity() - custom_cross(rB - B->get_center_of_mass(), B->get_angular_velocity()) - vA;
 	} else {
 		rel_vel = -vA;
 	}
@@ -213,8 +213,6 @@ PinJoint2DSW::PinJoint2DSW(const Vector2 &p_pos, Body2DSW *p_body_a, Body2DSW *p
 	anchor_A = p_body_a->get_inv_transform().xform(p_pos);
 	anchor_B = p_body_b ? p_body_b->get_inv_transform().xform(p_pos) : p_pos;
 
-	softness = 0;
-
 	p_body_a->add_constraint(this, 0);
 	if (p_body_b) {
 		p_body_b->add_constraint(this, 1);
@@ -238,6 +236,9 @@ k_tensor(Body2DSW *a, Body2DSW *b, Vector2 r1, Vector2 r2, Vector2 *k1, Vector2
 	k21 = 0.0f;
 	k22 = m_sum;
 
+	r1 -= a->get_center_of_mass();
+	r2 -= b->get_center_of_mass();
+
 	// add the influence from r1
 	real_t a_i_inv = a->get_inv_inertia();
 	real_t r1xsq = r1.x * r1.x * a_i_inv;
@@ -479,8 +480,6 @@ DampedSpringJoint2DSW::DampedSpringJoint2DSW(const Vector2 &p_anchor_a, const Ve
 	anchor_B = B->get_inv_transform().xform(p_anchor_b);
 
 	rest_length = p_anchor_a.distance_to(p_anchor_b);
-	stiffness = 20;
-	damping = 1.5;
 
 	A->add_constraint(this, 0);
 	B->add_constraint(this, 1);