1 files changed, 18 insertions, 40 deletions

diff --git a/servers/physics_2d/space_2d_sw.cpp b/servers/physics_2d/space_2d_sw.cpp
index 6c23f49928..b9b26eb21d 100644
--- a/servers/physics_2d/space_2d_sw.cpp
+++ b/servers/physics_2d/space_2d_sw.cpp
@@ -383,18 +383,18 @@ bool PhysicsDirectSpaceState2DSW::collide_shape(RID p_shape, const Transform2D &
 }
 
 struct _RestCallbackData2D {
-	const CollisionObject2DSW *object;
-	const CollisionObject2DSW *best_object;
-	int local_shape;
-	int best_local_shape;
-	int shape;
-	int best_shape;
+	const CollisionObject2DSW *object = nullptr;
+	const CollisionObject2DSW *best_object = nullptr;
+	int local_shape = 0;
+	int best_local_shape = 0;
+	int shape = 0;
+	int best_shape = 0;
 	Vector2 best_contact;
 	Vector2 best_normal;
-	real_t best_len;
+	real_t best_len = 0.0;
 	Vector2 valid_dir;
-	real_t valid_depth;
-	real_t min_allowed_depth;
+	real_t valid_depth = 0.0;
+	real_t min_allowed_depth = 0.0;
 };
 
 static void _rest_cbk_result(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_userdata) {
@@ -482,7 +482,7 @@ bool PhysicsDirectSpaceState2DSW::rest_info(RID p_shape, const Transform2D &p_sh
 	r_info->metadata = rcd.best_object->get_shape_metadata(rcd.best_shape);
 	if (rcd.best_object->get_type() == CollisionObject2DSW::TYPE_BODY) {
 		const Body2DSW *body = static_cast<const Body2DSW *>(rcd.best_object);
-		Vector2 rel_vec = r_info->point - body->get_transform().get_origin();
+		Vector2 rel_vec = r_info->point - (body->get_transform().get_origin() + body->get_center_of_mass());
 		r_info->linear_velocity = Vector2(-body->get_angular_velocity() * rel_vec.y, body->get_angular_velocity() * rel_vec.x) + body->get_linear_velocity();
 
 	} else {
@@ -492,10 +492,6 @@ bool PhysicsDirectSpaceState2DSW::rest_info(RID p_shape, const Transform2D &p_sh
 	return true;
 }
 
-PhysicsDirectSpaceState2DSW::PhysicsDirectSpaceState2DSW() {
-	space = nullptr;
-}
-
 ////////////////////////////////////////////////////////////////////////////////////////////////////////////
 
 int Space2DSW::_cull_aabb_for_body(Body2DSW *p_body, const Rect2 &p_aabb) {
@@ -961,7 +957,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
 				r_result->collider_metadata = rcd.best_object->get_shape_metadata(rcd.best_shape);
 
 				const Body2DSW *body = static_cast<const Body2DSW *>(rcd.best_object);
-				Vector2 rel_vec = r_result->collision_point - body->get_transform().get_origin();
+				Vector2 rel_vec = r_result->collision_point - (body->get_transform().get_origin() + body->get_center_of_mass());
 				r_result->collider_velocity = Vector2(-body->get_angular_velocity() * rel_vec.y, body->get_angular_velocity() * rel_vec.x) + body->get_linear_velocity();
 
 				r_result->travel = safe * p_motion;
@@ -1041,12 +1037,12 @@ void Space2DSW::body_remove_from_active_list(SelfList<Body2DSW> *p_body) {
 	active_list.remove(p_body);
 }
 
-void Space2DSW::body_add_to_inertia_update_list(SelfList<Body2DSW> *p_body) {
-	inertia_update_list.add(p_body);
+void Space2DSW::body_add_to_mass_properties_update_list(SelfList<Body2DSW> *p_body) {
+	mass_properties_update_list.add(p_body);
 }
 
-void Space2DSW::body_remove_from_inertia_update_list(SelfList<Body2DSW> *p_body) {
-	inertia_update_list.remove(p_body);
+void Space2DSW::body_remove_from_mass_properties_update_list(SelfList<Body2DSW> *p_body) {
+	mass_properties_update_list.remove(p_body);
 }
 
 BroadPhase2DSW *Space2DSW::get_broadphase() {
@@ -1112,9 +1108,9 @@ void Space2DSW::call_queries() {
 void Space2DSW::setup() {
 	contact_debug_count = 0;
 
-	while (inertia_update_list.first()) {
-		inertia_update_list.first()->self()->update_inertias();
-		inertia_update_list.remove(inertia_update_list.first());
+	while (mass_properties_update_list.first()) {
+		mass_properties_update_list.first()->self()->update_mass_properties();
+		mass_properties_update_list.remove(mass_properties_update_list.first());
 	}
 }
 
@@ -1190,19 +1186,6 @@ PhysicsDirectSpaceState2DSW *Space2DSW::get_direct_state() {
 }
 
 Space2DSW::Space2DSW() {
-	collision_pairs = 0;
-	active_objects = 0;
-	island_count = 0;
-
-	contact_debug_count = 0;
-
-	locked = false;
-	contact_recycle_radius = 1.0;
-	contact_max_separation = 1.5;
-	contact_max_allowed_penetration = 0.3;
-	test_motion_min_contact_depth = 0.005;
-
-	constraint_bias = 0.2;
 	body_linear_velocity_sleep_threshold = GLOBAL_DEF("physics/2d/sleep_threshold_linear", 2.0);
 	body_angular_velocity_sleep_threshold = GLOBAL_DEF("physics/2d/sleep_threshold_angular", Math::deg2rad(8.0));
 	body_time_to_sleep = GLOBAL_DEF("physics/2d/time_before_sleep", 0.5);
@@ -1211,14 +1194,9 @@ Space2DSW::Space2DSW() {
 	broadphase = BroadPhase2DSW::create_func();
 	broadphase->set_pair_callback(_broadphase_pair, this);
 	broadphase->set_unpair_callback(_broadphase_unpair, this);
-	area = nullptr;
 
 	direct_access = memnew(PhysicsDirectSpaceState2DSW);
 	direct_access->space = this;
-
-	for (int i = 0; i < ELAPSED_TIME_MAX; i++) {
-		elapsed_time[i] = 0;
-	}
 }
 
 Space2DSW::~Space2DSW() {