1 files changed, 60 insertions, 84 deletions

diff --git a/servers/physics_3d/body_3d_sw.cpp b/servers/physics_3d/body_3d_sw.cpp
index fea5aed6ad..d1f16cb4ae 100644
--- a/servers/physics_3d/body_3d_sw.cpp
+++ b/servers/physics_3d/body_3d_sw.cpp
@@ -33,13 +33,12 @@
 #include "space_3d_sw.h"
 
 void Body3DSW::_update_inertia() {
-
-	if (get_space() && !inertia_update_list.in_list())
+	if (get_space() && !inertia_update_list.in_list()) {
 		get_space()->body_add_to_inertia_update_list(&inertia_update_list);
+	}
 }
 
 void Body3DSW::_update_transform_dependant() {
-
 	center_of_mass = get_transform().basis.xform(center_of_mass_local);
 	principal_inertia_axes = get_transform().basis * principal_inertia_axes_local;
 
@@ -52,18 +51,14 @@ void Body3DSW::_update_transform_dependant() {
 }
 
 void Body3DSW::update_inertias() {
-
 	//update shapes and motions
 
 	switch (mode) {
-
 		case PhysicsServer3D::BODY_MODE_RIGID: {
-
 			//update tensor for all shapes, not the best way but should be somehow OK. (inspired from bullet)
 			real_t total_area = 0;
 
 			for (int i = 0; i < get_shape_count(); i++) {
-
 				total_area += get_shape_area(i);
 			}
 
@@ -86,7 +81,6 @@ void Body3DSW::update_inertias() {
 			inertia_tensor.set_zero();
 
 			for (int i = 0; i < get_shape_count(); i++) {
-
 				if (is_shape_disabled(i)) {
 					continue;
 				}
@@ -112,21 +106,20 @@ void Body3DSW::update_inertias() {
 			principal_inertia_axes_local = inertia_tensor.diagonalize().transposed();
 			_inv_inertia = inertia_tensor.get_main_diagonal().inverse();
 
-			if (mass)
+			if (mass) {
 				_inv_mass = 1.0 / mass;
-			else
+			} else {
 				_inv_mass = 0;
+			}
 
 		} break;
 
 		case PhysicsServer3D::BODY_MODE_KINEMATIC:
 		case PhysicsServer3D::BODY_MODE_STATIC: {
-
 			_inv_inertia_tensor.set_zero();
 			_inv_mass = 0;
 		} break;
 		case PhysicsServer3D::BODY_MODE_CHARACTER: {
-
 			_inv_inertia_tensor.set_zero();
 			_inv_mass = 1.0 / mass;
 
@@ -139,19 +132,22 @@ void Body3DSW::update_inertias() {
 }
 
 void Body3DSW::set_active(bool p_active) {
-
-	if (active == p_active)
+	if (active == p_active) {
 		return;
+	}
 
 	active = p_active;
 	if (!p_active) {
-		if (get_space())
+		if (get_space()) {
 			get_space()->body_remove_from_active_list(&active_list);
+		}
 	} else {
-		if (mode == PhysicsServer3D::BODY_MODE_STATIC)
+		if (mode == PhysicsServer3D::BODY_MODE_STATIC) {
 			return; //static bodies can't become active
-		if (get_space())
+		}
+		if (get_space()) {
 			get_space()->body_add_to_active_list(&active_list);
+		}
 
 		//still_time=0;
 	}
@@ -169,14 +165,11 @@ void Body3DSW::set_active(bool p_active) {
 }
 
 void Body3DSW::set_param(PhysicsServer3D::BodyParameter p_param, real_t p_value) {
-
 	switch (p_param) {
 		case PhysicsServer3D::BODY_PARAM_BOUNCE: {
-
 			bounce = p_value;
 		} break;
 		case PhysicsServer3D::BODY_PARAM_FRICTION: {
-
 			friction = p_value;
 		} break;
 		case PhysicsServer3D::BODY_PARAM_MASS: {
@@ -189,11 +182,9 @@ void Body3DSW::set_param(PhysicsServer3D::BodyParameter p_param, real_t p_value)
 			gravity_scale = p_value;
 		} break;
 		case PhysicsServer3D::BODY_PARAM_LINEAR_DAMP: {
-
 			linear_damp = p_value;
 		} break;
 		case PhysicsServer3D::BODY_PARAM_ANGULAR_DAMP: {
-
 			angular_damp = p_value;
 		} break;
 		default: {
@@ -202,14 +193,11 @@ void Body3DSW::set_param(PhysicsServer3D::BodyParameter p_param, real_t p_value)
 }
 
 real_t Body3DSW::get_param(PhysicsServer3D::BodyParameter p_param) const {
-
 	switch (p_param) {
 		case PhysicsServer3D::BODY_PARAM_BOUNCE: {
-
 			return bounce;
 		} break;
 		case PhysicsServer3D::BODY_PARAM_FRICTION: {
-
 			return friction;
 		} break;
 		case PhysicsServer3D::BODY_PARAM_MASS: {
@@ -219,11 +207,9 @@ real_t Body3DSW::get_param(PhysicsServer3D::BodyParameter p_param) const {
 			return gravity_scale;
 		} break;
 		case PhysicsServer3D::BODY_PARAM_LINEAR_DAMP: {
-
 			return linear_damp;
 		} break;
 		case PhysicsServer3D::BODY_PARAM_ANGULAR_DAMP: {
-
 			return angular_damp;
 		} break;
 
@@ -235,7 +221,6 @@ real_t Body3DSW::get_param(PhysicsServer3D::BodyParameter p_param) const {
 }
 
 void Body3DSW::set_mode(PhysicsServer3D::BodyMode p_mode) {
-
 	PhysicsServer3D::BodyMode prev = mode;
 	mode = p_mode;
 
@@ -243,7 +228,6 @@ void Body3DSW::set_mode(PhysicsServer3D::BodyMode p_mode) {
 		//CLEAR UP EVERYTHING IN CASE IT NOT WORKS!
 		case PhysicsServer3D::BODY_MODE_STATIC:
 		case PhysicsServer3D::BODY_MODE_KINEMATIC: {
-
 			_set_inv_transform(get_transform().affine_inverse());
 			_inv_mass = 0;
 			_set_static(p_mode == PhysicsServer3D::BODY_MODE_STATIC);
@@ -257,14 +241,12 @@ void Body3DSW::set_mode(PhysicsServer3D::BodyMode p_mode) {
 
 		} break;
 		case PhysicsServer3D::BODY_MODE_RIGID: {
-
 			_inv_mass = mass > 0 ? (1.0 / mass) : 0;
 			_set_static(false);
 			set_active(true);
 
 		} break;
 		case PhysicsServer3D::BODY_MODE_CHARACTER: {
-
 			_inv_mass = mass > 0 ? (1.0 / mass) : 0;
 			_set_static(false);
 			set_active(true);
@@ -278,21 +260,18 @@ void Body3DSW::set_mode(PhysicsServer3D::BodyMode p_mode) {
 		_update_queries();
 	*/
 }
-PhysicsServer3D::BodyMode Body3DSW::get_mode() const {
 
+PhysicsServer3D::BodyMode Body3DSW::get_mode() const {
 	return mode;
 }
 
 void Body3DSW::_shapes_changed() {
-
 	_update_inertia();
 }
 
 void Body3DSW::set_state(PhysicsServer3D::BodyState p_state, const Variant &p_variant) {
-
 	switch (p_state) {
 		case PhysicsServer3D::BODY_STATE_TRANSFORM: {
-
 			if (mode == PhysicsServer3D::BODY_MODE_KINEMATIC) {
 				new_transform = p_variant;
 				//wakeup_neighbours();
@@ -311,8 +290,9 @@ void Body3DSW::set_state(PhysicsServer3D::BodyState p_state, const Variant &p_va
 				Transform t = p_variant;
 				t.orthonormalize();
 				new_transform = get_transform(); //used as old to compute motion
-				if (new_transform == t)
+				if (new_transform == t) {
 					break;
+				}
 				_set_transform(t);
 				_set_inv_transform(get_transform().inverse());
 			}
@@ -320,7 +300,6 @@ void Body3DSW::set_state(PhysicsServer3D::BodyState p_state, const Variant &p_va
 
 		} break;
 		case PhysicsServer3D::BODY_STATE_LINEAR_VELOCITY: {
-
 			/*
 			if (mode==PhysicsServer3D::BODY_MODE_STATIC)
 				break;
@@ -339,8 +318,9 @@ void Body3DSW::set_state(PhysicsServer3D::BodyState p_state, const Variant &p_va
 		} break;
 		case PhysicsServer3D::BODY_STATE_SLEEPING: {
 			//?
-			if (mode == PhysicsServer3D::BODY_MODE_STATIC || mode == PhysicsServer3D::BODY_MODE_KINEMATIC)
+			if (mode == PhysicsServer3D::BODY_MODE_STATIC || mode == PhysicsServer3D::BODY_MODE_KINEMATIC) {
 				break;
+			}
 			bool do_sleep = p_variant;
 			if (do_sleep) {
 				linear_velocity = Vector3();
@@ -354,14 +334,15 @@ void Body3DSW::set_state(PhysicsServer3D::BodyState p_state, const Variant &p_va
 		} break;
 		case PhysicsServer3D::BODY_STATE_CAN_SLEEP: {
 			can_sleep = p_variant;
-			if (mode == PhysicsServer3D::BODY_MODE_RIGID && !active && !can_sleep)
+			if (mode == PhysicsServer3D::BODY_MODE_RIGID && !active && !can_sleep) {
 				set_active(true);
+			}
 
 		} break;
 	}
 }
-Variant Body3DSW::get_state(PhysicsServer3D::BodyState p_state) const {
 
+Variant Body3DSW::get_state(PhysicsServer3D::BodyState p_state) const {
 	switch (p_state) {
 		case PhysicsServer3D::BODY_STATE_TRANSFORM: {
 			return get_transform();
@@ -384,24 +365,25 @@ Variant Body3DSW::get_state(PhysicsServer3D::BodyState p_state) const {
 }
 
 void Body3DSW::set_space(Space3DSW *p_space) {
-
 	if (get_space()) {
-
-		if (inertia_update_list.in_list())
+		if (inertia_update_list.in_list()) {
 			get_space()->body_remove_from_inertia_update_list(&inertia_update_list);
-		if (active_list.in_list())
+		}
+		if (active_list.in_list()) {
 			get_space()->body_remove_from_active_list(&active_list);
-		if (direct_state_query_list.in_list())
+		}
+		if (direct_state_query_list.in_list()) {
 			get_space()->body_remove_from_state_query_list(&direct_state_query_list);
+		}
 	}
 
 	_set_space(p_space);
 
 	if (get_space()) {
-
 		_update_inertia();
-		if (active)
+		if (active) {
 			get_space()->body_add_to_active_list(&active_list);
+		}
 		/*
 		_update_queries();
 		if (is_active()) {
@@ -415,7 +397,6 @@ void Body3DSW::set_space(Space3DSW *p_space) {
 }
 
 void Body3DSW::_compute_area_gravity_and_dampenings(const Area3DSW *p_area) {
-
 	if (p_area->is_gravity_point()) {
 		if (p_area->get_gravity_distance_scale() > 0) {
 			Vector3 v = p_area->get_transform().xform(p_area->get_gravity_vector()) - get_transform().get_origin();
@@ -444,9 +425,9 @@ bool Body3DSW::is_axis_locked(PhysicsServer3D::BodyAxis p_axis) const {
 }
 
 void Body3DSW::integrate_forces(real_t p_step) {
-
-	if (mode == PhysicsServer3D::BODY_MODE_STATIC)
+	if (mode == PhysicsServer3D::BODY_MODE_STATIC) {
 		return;
+	}
 
 	Area3DSW *def_area = get_space()->get_default_area();
 	// AreaSW *damp_area = def_area;
@@ -491,15 +472,17 @@ void Body3DSW::integrate_forces(real_t p_step) {
 	gravity *= gravity_scale;
 
 	// If less than 0, override dampenings with that of the Body
-	if (angular_damp >= 0)
+	if (angular_damp >= 0) {
 		area_angular_damp = angular_damp;
+	}
 	/*
 	else
 		area_angular_damp=damp_area->get_angular_damp();
 	*/
 
-	if (linear_damp >= 0)
+	if (linear_damp >= 0) {
 		area_linear_damp = linear_damp;
+	}
 	/*
 	else
 		area_linear_damp=damp_area->get_linear_damp();
@@ -509,7 +492,6 @@ void Body3DSW::integrate_forces(real_t p_step) {
 	bool do_motion = false;
 
 	if (mode == PhysicsServer3D::BODY_MODE_KINEMATIC) {
-
 		//compute motion, angular and etc. velocities from prev transform
 		linear_velocity = (new_transform.origin - get_transform().origin) / p_step;
 
@@ -535,13 +517,15 @@ void Body3DSW::integrate_forces(real_t p_step) {
 
 			real_t damp = 1.0 - p_step * area_linear_damp;
 
-			if (damp < 0) // reached zero in the given time
+			if (damp < 0) { // reached zero in the given time
 				damp = 0;
+			}
 
 			real_t angular_damp = 1.0 - p_step * area_angular_damp;
 
-			if (angular_damp < 0) // reached zero in the given time
+			if (angular_damp < 0) { // reached zero in the given time
 				angular_damp = 0;
+			}
 
 			linear_velocity *= damp;
 			angular_velocity *= angular_damp;
@@ -574,12 +558,13 @@ void Body3DSW::integrate_forces(real_t p_step) {
 }
 
 void Body3DSW::integrate_velocities(real_t p_step) {
-
-	if (mode == PhysicsServer3D::BODY_MODE_STATIC)
+	if (mode == PhysicsServer3D::BODY_MODE_STATIC) {
 		return;
+	}
 
-	if (fi_callback)
+	if (fi_callback) {
 		get_space()->body_add_to_state_query_list(&direct_state_query_list);
+	}
 
 	//apply axis lock linear
 	for (int i = 0; i < 3; i++) {
@@ -598,11 +583,11 @@ void Body3DSW::integrate_velocities(real_t p_step) {
 	}
 
 	if (mode == PhysicsServer3D::BODY_MODE_KINEMATIC) {
-
 		_set_transform(new_transform, false);
 		_set_inv_transform(new_transform.affine_inverse());
-		if (contacts.size() == 0 && linear_velocity == Vector3() && angular_velocity == Vector3())
+		if (contacts.size() == 0 && linear_velocity == Vector3() && angular_velocity == Vector3()) {
 			set_active(false); //stopped moving, deactivate
+		}
 
 		return;
 	}
@@ -673,34 +658,33 @@ void BodySW::simulate_motion(const Transform& p_xform,real_t p_step) {
 
 
 }
+
 */
 
 void Body3DSW::wakeup_neighbours() {
-
 	for (Map<Constraint3DSW *, int>::Element *E = constraint_map.front(); E; E = E->next()) {
-
 		const Constraint3DSW *c = E->key();
 		Body3DSW **n = c->get_body_ptr();
 		int bc = c->get_body_count();
 
 		for (int i = 0; i < bc; i++) {
-
-			if (i == E->get())
+			if (i == E->get()) {
 				continue;
+			}
 			Body3DSW *b = n[i];
-			if (b->mode != PhysicsServer3D::BODY_MODE_RIGID)
+			if (b->mode != PhysicsServer3D::BODY_MODE_RIGID) {
 				continue;
+			}
 
-			if (!b->is_active())
+			if (!b->is_active()) {
 				b->set_active(true);
+			}
 		}
 	}
 }
 
 void Body3DSW::call_queries() {
-
 	if (fi_callback) {
-
 		PhysicsDirectBodyState3DSW *dbs = PhysicsDirectBodyState3DSW::singleton;
 		dbs->body = this;
 
@@ -708,7 +692,6 @@ void Body3DSW::call_queries() {
 
 		Object *obj = ObjectDB::get_instance(fi_callback->id);
 		if (!obj) {
-
 			set_force_integration_callback(ObjectID(), StringName());
 		} else {
 			const Variant *vp[2] = { &v, &fi_callback->udata };
@@ -721,36 +704,31 @@ void Body3DSW::call_queries() {
 }
 
 bool Body3DSW::sleep_test(real_t p_step) {
-
-	if (mode == PhysicsServer3D::BODY_MODE_STATIC || mode == PhysicsServer3D::BODY_MODE_KINEMATIC)
+	if (mode == PhysicsServer3D::BODY_MODE_STATIC || mode == PhysicsServer3D::BODY_MODE_KINEMATIC) {
 		return true; //
-	else if (mode == PhysicsServer3D::BODY_MODE_CHARACTER)
+	} else if (mode == PhysicsServer3D::BODY_MODE_CHARACTER) {
 		return !active; // characters don't sleep unless asked to sleep
-	else if (!can_sleep)
+	} else if (!can_sleep) {
 		return false;
+	}
 
 	if (Math::abs(angular_velocity.length()) < get_space()->get_body_angular_velocity_sleep_threshold() && Math::abs(linear_velocity.length_squared()) < get_space()->get_body_linear_velocity_sleep_threshold() * get_space()->get_body_linear_velocity_sleep_threshold()) {
-
 		still_time += p_step;
 
 		return still_time > get_space()->get_body_time_to_sleep();
 	} else {
-
 		still_time = 0; //maybe this should be set to 0 on set_active?
 		return false;
 	}
 }
 
 void Body3DSW::set_force_integration_callback(ObjectID p_id, const StringName &p_method, const Variant &p_udata) {
-
 	if (fi_callback) {
-
 		memdelete(fi_callback);
 		fi_callback = nullptr;
 	}
 
 	if (p_id.is_valid()) {
-
 		fi_callback = memnew(ForceIntegrationCallback);
 		fi_callback->id = p_id;
 		fi_callback->method = p_method;
@@ -764,16 +742,15 @@ void Body3DSW::set_kinematic_margin(real_t p_margin) {
 
 Body3DSW::Body3DSW() :
 		CollisionObject3DSW(TYPE_BODY),
-		locked_axis(0),
+
 		active_list(this),
 		inertia_update_list(this),
 		direct_state_query_list(this) {
-
 	mode = PhysicsServer3D::BODY_MODE_RIGID;
 	active = true;
 
 	mass = 1;
-	kinematic_safe_margin = 0.01;
+	kinematic_safe_margin = 0.001;
 	//_inv_inertia=Transform();
 	_inv_mass = 1;
 	bounce = 0;
@@ -801,14 +778,13 @@ Body3DSW::Body3DSW() :
 }
 
 Body3DSW::~Body3DSW() {
-
-	if (fi_callback)
+	if (fi_callback) {
 		memdelete(fi_callback);
+	}
 }
 
 PhysicsDirectBodyState3DSW *PhysicsDirectBodyState3DSW::singleton = nullptr;
 
 PhysicsDirectSpaceState3D *PhysicsDirectBodyState3DSW::get_space_state() {
-
 	return body->get_space()->get_direct_state();
 }