Merge pull request #7445 from tagcup/2d_math_fixes

Various corrections in 2D math.

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;