Various corrections in 2D math.

This is the follow up for the 2D changes mentioned in PR #6865. It fixes various mistakes regarding the order of matrix indices, order of transformation operations, usage of atan2 function and ensures that the sense of rotation is compatible with a left-handed coordinate system with Y-axis pointing down (which flips the sense of rotations along the z-axis). Also replaced float with real_t, and tried to make use of Matrix32 methods rather than accessing its elements directly.

Affected code in the Godot code base is also fixed in this commit.

The user code using functions involving angles such as atan2, angle_to, get_rotation, set_rotation will need to be updated to conform with the new behavior. Furthermore, the sign of the rotation angles in existing 2D scene files need to be flipped as well.

5 files changed, 15 insertions, 14 deletions

diff --git a/servers/physics_2d/body_2d_sw.cpp b/servers/physics_2d/body_2d_sw.cpp
index 0e3a0b7a71..9d859c1107 100644
--- a/servers/physics_2d/body_2d_sw.cpp
+++ b/servers/physics_2d/body_2d_sw.cpp
@@ -473,12 +473,13 @@ void Body2DSW::integrate_forces(real_t p_step) {
 	if (mode==Physics2DServer::BODY_MODE_KINEMATIC) {
 
 		//compute motion, angular and etc. velocities from prev transform
-		linear_velocity = (new_transform.elements[2] - get_transform().elements[2])/p_step;
+		motion = new_transform.get_origin() - get_transform().get_origin();
+		linear_velocity = motion/p_step;
 
-		real_t rot = new_transform.affine_inverse().basis_xform(get_transform().elements[1]).angle();
+		real_t rot = new_transform.get_rotation() - get_transform().get_rotation();
 		angular_velocity = rot / p_step;
 
-		motion = new_transform.elements[2] - get_transform().elements[2];
+
 		do_motion=true;
 
 		//for(int i=0;i<get_shape_count();i++) {
@@ -556,7 +557,7 @@ void Body2DSW::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;
 
-	real_t angle = get_transform().get_rotation() - total_angular_velocity * p_step;
+	real_t angle = get_transform().get_rotation() + total_angular_velocity * p_step;
 	Vector2 pos = get_transform().get_origin() + total_linear_velocity * p_step;
 
 	_set_transform(Matrix32(angle,pos),continuous_cd_mode==Physics2DServer::CCD_MODE_DISABLED);
diff --git a/servers/physics_2d/body_pair_2d_sw.cpp b/servers/physics_2d/body_pair_2d_sw.cpp
index 6fa4f154d1..7c00f22dfd 100644
--- a/servers/physics_2d/body_pair_2d_sw.cpp
+++ b/servers/physics_2d/body_pair_2d_sw.cpp
@@ -249,7 +249,7 @@ bool BodyPair2DSW::setup(float p_step) {
 	Matrix32 xform_A = xform_Au * A->get_shape_transform(shape_A);
 
 	Matrix32 xform_Bu = B->get_transform();
-	xform_Bu.elements[2]-=A->get_transform().get_origin();
+	xform_Bu.translate(-A->get_transform().get_origin());
 	Matrix32 xform_B = xform_Bu * B->get_shape_transform(shape_B);
 
 	Shape2DSW *shape_A_ptr=A->get_shape(shape_A);
diff --git a/servers/physics_2d/collision_solver_2d_sw.cpp b/servers/physics_2d/collision_solver_2d_sw.cpp
index 0ad519c9d1..079b0499c2 100644
--- a/servers/physics_2d/collision_solver_2d_sw.cpp
+++ b/servers/physics_2d/collision_solver_2d_sw.cpp
@@ -203,14 +203,14 @@ bool CollisionSolver2DSW::solve_concave(const Shape2DSW *p_shape_A,const Matrix3
 	cinfo.aabb_tests=0;
 
 	Matrix32 rel_transform = p_transform_A;
-	rel_transform.elements[2]-=p_transform_B.elements[2];
+	rel_transform.translate(-p_transform_B.get_origin());
 
 	//quickly compute a local Rect2
 
 	Rect2 local_aabb;
 	for(int i=0;i<2;i++) {
 
-	     Vector2 axis( p_transform_B.elements[i] );
+	     Vector2 axis( p_transform_B.get_axis(i) );
 	     float axis_scale = 1.0/axis.length();
 	     axis*=axis_scale;
 
diff --git a/servers/physics_2d/shape_2d_sw.h b/servers/physics_2d/shape_2d_sw.h
index a3e4ef94b3..a955cb1eee 100644
--- a/servers/physics_2d/shape_2d_sw.h
+++ b/servers/physics_2d/shape_2d_sw.h
@@ -150,7 +150,7 @@ _FORCE_INLINE_ void project_range_cast(const Vector2& p_cast, const Vector2& p_n
 	real_t mina,maxa;\
 	real_t minb,maxb;\
 	Matrix32 ofsb=p_transform;\
-	ofsb.elements[2]+=p_cast;\
+	ofsb.translate(p_cast);\
 	project_range(p_normal,p_transform,mina,maxa);\
 	project_range(p_normal,ofsb,minb,maxb);	\
 	r_min=MIN(mina,minb);\
diff --git a/servers/physics_2d/space_2d_sw.cpp b/servers/physics_2d/space_2d_sw.cpp
index 8b277aa4b9..7985bfef96 100644
--- a/servers/physics_2d/space_2d_sw.cpp
+++ b/servers/physics_2d/space_2d_sw.cpp
@@ -711,7 +711,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Matrix32 &p_from, const
 				break;
 			}
 
-			body_transform.elements[2]+=recover_motion;
+			body_transform.translate(recover_motion);
 			body_aabb.pos+=recover_motion;
 
 			recover_attempts--;
@@ -852,15 +852,15 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Matrix32 &p_from, const
 		if (r_result) {
 
 			r_result->motion=p_motion;
-			r_result->remainder=Vector2();			
-			r_result->motion+=(body_transform.elements[2]-p_from.elements[2]);
+			r_result->remainder=Vector2();
+			r_result->motion+=(body_transform.get_origin()-p_from.get_origin());
 		}
 
 	} else {
 
 		//it collided, let's get the rest info in unsafe advance
 		Matrix32 ugt = body_transform;
-		ugt.elements[2]+=p_motion*unsafe;
+		ugt.translate(p_motion*unsafe);
 
 		_RestCallbackData2D rcd;
 		rcd.best_len=0;
@@ -916,7 +916,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Matrix32 &p_from, const
 
 				r_result->motion=safe*p_motion;
 				r_result->remainder=p_motion - safe * p_motion;
-				r_result->motion+=(body_transform.elements[2]-p_from.elements[2]);
+				r_result->motion+=(body_transform.get_origin()-p_from.get_origin());
 
 			}
 
@@ -926,7 +926,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Matrix32 &p_from, const
 
 				r_result->motion=p_motion;
 				r_result->remainder=Vector2();
-				r_result->motion+=(body_transform.elements[2]-p_from.elements[2]);
+				r_result->motion+=(body_transform.get_origin()-p_from.get_origin());
 			}
 
 			collided=false;