Rigidbody 2D support scale & skew transformation

modules/godot_physics_2d/godot_body_2d.cpp

@@ -316,13 +316,12 @@ void GodotBody2D::set_state(PhysicsServer2D::BodyState p_state, const Variant &p
 				wakeup_neighbours();
 			} else {
 				Transform2D t = p_variant;
-				t.orthonormalize();
 				new_transform = get_transform(); //used as old to compute motion
 				if (t == new_transform) {
 					break;
 				}
 				_set_transform(t);
-				_set_inv_transform(get_transform().inverse());
+				_set_inv_transform(t.inverse());
 				_update_transform_dependent();
 			}
 			wakeup();
@@ -580,11 +579,7 @@ void GodotBody2D::integrate_forces(real_t p_step) {
 				damp = 0;
 			}
 
-			real_t angular_damp_new = 1.0 - p_step * total_angular_damp;
-
-			if (angular_damp_new < 0) { // reached zero in the given time
-				angular_damp_new = 0;
-			}
+			real_t angular_damp_new = MAX(0, 1.0 - p_step * total_angular_damp);
 
 			linear_velocity *= damp;
 			angular_velocity *= angular_damp_new;
@@ -634,21 +629,25 @@ void GodotBody2D::integrate_velocities(real_t p_step) {
 
 	real_t total_angular_velocity = angular_velocity + biased_angular_velocity;
 	Vector2 total_linear_velocity = linear_velocity + biased_linear_velocity;
+	Transform2D old_transform = get_transform();
 
 	real_t angle_delta = total_angular_velocity * p_step;
-	real_t angle = get_transform().get_rotation() + angle_delta;
-	Vector2 pos = get_transform().get_origin() + total_linear_velocity * p_step;
+	Transform2D rot = Transform2D(angle_delta, Vector2());
+	Vector2 pos_delta = total_linear_velocity * p_step;
 
 	if (center_of_mass.length_squared() > CMP_EPSILON2) {
 		// Calculate displacement due to center of mass offset.
-		pos += center_of_mass - center_of_mass.rotated(angle_delta);
+		pos_delta += center_of_mass - rot.xform(center_of_mass);
 	}
+	Vector2 pos = old_transform.get_origin() + pos_delta;
+	old_transform = rot * old_transform;
+	old_transform.set_origin(pos);
 
-	_set_transform(Transform2D(angle, pos), continuous_cd_mode == PhysicsServer2D::CCD_MODE_DISABLED);
+	_set_transform(old_transform, continuous_cd_mode == PhysicsServer2D::CCD_MODE_DISABLED);
 	_set_inv_transform(get_transform().inverse());
 
 	if (continuous_cd_mode != PhysicsServer2D::CCD_MODE_DISABLED) {
-		new_transform = get_transform();
+		old_transform = get_transform();
 	}
 
 	_update_transform_dependent();

modules/godot_physics_2d/godot_body_pair_2d.cpp

@@ -45,26 +45,24 @@ void GodotBodyPair2D::_add_contact(const Vector2 &p_point_A, const Vector2 &p_po
 }
 
 void GodotBodyPair2D::_contact_added_callback(const Vector2 &p_point_A, const Vector2 &p_point_B) {
-	Vector2 local_A = A->get_inv_transform().basis_xform(p_point_A);
-	Vector2 local_B = B->get_inv_transform().basis_xform(p_point_B - offset_B);
-
 	int new_index = contact_count;
 
 	ERR_FAIL_COND(new_index >= (MAX_CONTACTS + 1));
 
 	Contact contact;
-	contact.local_A = local_A;
-	contact.local_B = local_B;
-	contact.normal = (p_point_A - p_point_B).normalized();
+	contact.position_A = p_point_A;
+	contact.position_B = p_point_B;
+	contact.axis = p_point_A - p_point_B;
+	contact.normal = contact.axis.normalized();
 	contact.used = true;
 
 	// Attempt to determine if the contact will be reused.
 	real_t recycle_radius_2 = space->get_contact_recycle_radius() * space->get_contact_recycle_radius();
 
 	for (int i = 0; i < contact_count; i++) {
 		Contact &c = contacts[i];
-		if (c.local_A.distance_squared_to(local_A) < (recycle_radius_2) &&
-				c.local_B.distance_squared_to(local_B) < (recycle_radius_2)) {
+		if (c.position_A.distance_squared_to(p_point_A) < (recycle_radius_2) &&
+				c.position_B.distance_squared_to(p_point_B) < (recycle_radius_2)) {
 			contact.acc_normal_impulse = c.acc_normal_impulse;
 			contact.acc_tangent_impulse = c.acc_tangent_impulse;
 			contact.acc_bias_impulse = c.acc_bias_impulse;
@@ -77,29 +75,17 @@ void GodotBodyPair2D::_contact_added_callback(const Vector2 &p_point_A, const Ve
 	// Figure out if the contact amount must be reduced to fit the new contact.
 	if (new_index == MAX_CONTACTS) {
 		// Remove the contact with the minimum depth.
-
-		const Transform2D &transform_A = A->get_transform();
-		const Transform2D &transform_B = B->get_transform();
-
 		int least_deep = -1;
 		real_t min_depth;
 
 		// Start with depth for new contact.
 		{
-			Vector2 global_A = transform_A.basis_xform(contact.local_A);
-			Vector2 global_B = transform_B.basis_xform(contact.local_B) + offset_B;
-
-			Vector2 axis = global_A - global_B;
-			min_depth = axis.dot(contact.normal);
+			min_depth = contact.axis.dot(contact.normal);
 		}
 
 		for (int i = 0; i < contact_count; i++) {
 			const Contact &c = contacts[i];
-			Vector2 global_A = transform_A.basis_xform(c.local_A);
-			Vector2 global_B = transform_B.basis_xform(c.local_B) + offset_B;
-
-			Vector2 axis = global_A - global_B;
-			real_t depth = axis.dot(c.normal);
+			real_t depth = c.axis.dot(c.normal);
 
 			if (depth < min_depth) {
 				min_depth = depth;
@@ -124,9 +110,6 @@ void GodotBodyPair2D::_validate_contacts() {
 	real_t max_separation = space->get_contact_max_separation();
 	real_t max_separation2 = max_separation * max_separation;
 
-	const Transform2D &transform_A = A->get_transform();
-	const Transform2D &transform_B = B->get_transform();
-
 	for (int i = 0; i < contact_count; i++) {
 		Contact &c = contacts[i];
 
@@ -137,12 +120,9 @@ void GodotBodyPair2D::_validate_contacts() {
 		} else {
 			c.used = false;
 
-			Vector2 global_A = transform_A.basis_xform(c.local_A);
-			Vector2 global_B = transform_B.basis_xform(c.local_B) + offset_B;
-			Vector2 axis = global_A - global_B;
-			real_t depth = axis.dot(c.normal);
+			real_t depth = c.axis.dot(c.normal);
 
-			if (depth < -max_separation || (global_B + c.normal * depth - global_A).length_squared() > max_separation2) {
+			if (depth < -max_separation || (c.normal * depth - c.axis).length_squared() > max_separation2) {
 				erase = true;
 			}
 		}
@@ -409,9 +389,6 @@ bool GodotBodyPair2D::pre_solve(real_t p_step) {
 	bool do_process = false;
 
 	const Vector2 &offset_A = A->get_transform().get_origin();
-	const Transform2D &transform_A = A->get_transform();
-	const Transform2D &transform_B = B->get_transform();
-
 	real_t inv_inertia_A = collide_A ? A->get_inv_inertia() : 0.0;
 	real_t inv_inertia_B = collide_B ? B->get_inv_inertia() : 0.0;
 
@@ -422,25 +399,21 @@ bool GodotBodyPair2D::pre_solve(real_t p_step) {
 		Contact &c = contacts[i];
 		c.active = false;
 
-		Vector2 global_A = transform_A.basis_xform(c.local_A);
-		Vector2 global_B = transform_B.basis_xform(c.local_B) + offset_B;
-
-		Vector2 axis = global_A - global_B;
-		real_t depth = axis.dot(c.normal);
+		real_t depth = c.axis.dot(c.normal);
 
 		if (depth <= 0.0) {
 			continue;
 		}
 
 #ifdef DEBUG_ENABLED
 		if (space->is_debugging_contacts()) {
-			space->add_debug_contact(global_A + offset_A);
-			space->add_debug_contact(global_B + offset_A);
+			space->add_debug_contact(c.position_A + offset_A);
+			space->add_debug_contact(c.position_B + offset_A);
 		}
 #endif
 
-		c.rA = global_A - A->get_center_of_mass();
-		c.rB = global_B - B->get_center_of_mass() - offset_B;
+		c.rA = c.position_A - A->get_center_of_mass();
+		c.rB = c.position_B - B->get_center_of_mass() - offset_B;
 
 		// Precompute normal mass, tangent mass, and bias.
 		real_t rnA = c.rA.dot(c.normal);
@@ -467,10 +440,10 @@ bool GodotBodyPair2D::pre_solve(real_t p_step) {
 			Vector2 crB = Vector2(-B->get_angular_velocity() * c.rB.y, B->get_angular_velocity() * c.rB.x) + B->get_linear_velocity();
 			Vector2 crA = Vector2(-A->get_angular_velocity() * c.rA.y, A->get_angular_velocity() * c.rA.x) + A->get_linear_velocity();
 			if (A->can_report_contacts()) {
-				A->add_contact(global_A + offset_A, -c.normal, depth, shape_A, crA, global_B + offset_A, shape_B, B->get_instance_id(), B->get_self(), crB, c.acc_impulse);
+				A->add_contact(c.position_A + offset_A, -c.normal, depth, shape_A, crA, c.position_B + offset_A, shape_B, B->get_instance_id(), B->get_self(), crB, c.acc_impulse);
 			}
 			if (B->can_report_contacts()) {
-				B->add_contact(global_B + offset_A, c.normal, depth, shape_B, crB, global_A + offset_A, shape_A, A->get_instance_id(), A->get_self(), crA, c.acc_impulse);
+				B->add_contact(c.position_B + offset_A, c.normal, depth, shape_B, crB, c.position_A + offset_A, shape_A, A->get_instance_id(), A->get_self(), crA, c.acc_impulse);
 			}
 		}
 

modules/godot_physics_2d/godot_body_pair_2d.h

@@ -57,7 +57,8 @@ class GodotBodyPair2D : public GodotConstraint2D {
 	struct Contact {
 		Vector2 position;
 		Vector2 normal;
-		Vector2 local_A, local_B;
+		Vector2 axis;
+		Vector2 position_A, position_B;
 		Vector2 acc_impulse; // accumulated impulse
 		real_t acc_normal_impulse = 0.0; // accumulated normal impulse (Pn)
 		real_t acc_tangent_impulse = 0.0; // accumulated tangent impulse (Pt)

scene/2d/physics/collision_shape_2d.cpp

@@ -72,13 +72,11 @@ void CollisionShape2D::_notification(int p_what) {
 				_update_in_shape_owner();
 			}
 		} break;
-
-		case NOTIFICATION_LOCAL_TRANSFORM_CHANGED: {
+		case NOTIFICATION_TRANSFORM_CHANGED: {
 			if (collision_object) {
 				_update_in_shape_owner(true);
 			}
 		} break;
-
 		case NOTIFICATION_UNPARENTED: {
 			if (collision_object) {
 				collision_object->remove_shape_owner(owner_id);
@@ -324,7 +322,7 @@ void CollisionShape2D::_bind_methods() {
 }
 
 CollisionShape2D::CollisionShape2D() {
-	set_notify_local_transform(true);
+	set_notify_transform(true);
 	set_hide_clip_children(true);
 	debug_color = _get_default_debug_color();
 }

scene/2d/physics/physical_bone_2d.cpp

@@ -108,7 +108,7 @@ void PhysicalBone2D::_find_joint_child() {
 }
 
 PackedStringArray PhysicalBone2D::get_configuration_warnings() const {
-	PackedStringArray warnings = RigidBody2D::get_configuration_warnings();
+	PackedStringArray warnings = PhysicsBody2D::get_configuration_warnings();
 
 	if (!parent_skeleton) {
 		warnings.push_back(RTR("A PhysicalBone2D only works with a Skeleton2D or another PhysicalBone2D as a parent node!"));

scene/2d/physics/rigid_body_2d.cpp

@@ -645,18 +645,6 @@ void RigidBody2D::_notification(int p_what) {
 #endif
 }
 
-PackedStringArray RigidBody2D::get_configuration_warnings() const {
-	Transform2D t = get_transform();
-
-	PackedStringArray warnings = PhysicsBody2D::get_configuration_warnings();
-
-	if (Math::abs(t.columns[0].length() - 1.0) > 0.05 || Math::abs(t.columns[1].length() - 1.0) > 0.05) {
-		warnings.push_back(RTR("Size changes to RigidBody2D will be overridden by the physics engine when running.\nChange the size in children collision shapes instead."));
-	}
-
-	return warnings;
-}
-
 void RigidBody2D::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("set_mass", "mass"), &RigidBody2D::set_mass);
 	ClassDB::bind_method(D_METHOD("get_mass"), &RigidBody2D::get_mass);

scene/2d/physics/rigid_body_2d.h

@@ -234,8 +234,6 @@ class RigidBody2D : public PhysicsBody2D {
 
 	TypedArray<Node2D> get_colliding_bodies() const; //function for script
 
-	virtual PackedStringArray get_configuration_warnings() const override;
-
 	RigidBody2D();
 	~RigidBody2D();