diff --git a/core/math/math_2d.cpp b/core/math/math_2d.cpp
index 2ced18e427..df9383ed1b 100644
--- a/core/math/math_2d.cpp
+++ b/core/math/math_2d.cpp
@@ -31,22 +31,22 @@
 
 real_t Vector2::angle() const {
 
-	return Math::atan2(x,y);
+	return Math::atan2(y,x);
 }
 
-float Vector2::length() const {
+real_t Vector2::length() const {
 
 	return Math::sqrt( x*x + y*y );
 }
 
-float Vector2::length_squared() const {
+real_t Vector2::length_squared() const {
 
 	return x*x + y*y;
 }
 
 void Vector2::normalize() {
 
-	float l = x*x + y*y;
+	real_t l = x*x + y*y;
 	if (l!=0) {
 
 		l=Math::sqrt(l);
@@ -62,32 +62,32 @@ Vector2 Vector2::normalized() const {
 	return v;
 }
 
-float Vector2::distance_to(const Vector2& p_vector2) const {
+real_t Vector2::distance_to(const Vector2& p_vector2) const {
 
 	return Math::sqrt( (x-p_vector2.x)*(x-p_vector2.x) + (y-p_vector2.y)*(y-p_vector2.y));
 }
 
-float Vector2::distance_squared_to(const Vector2& p_vector2) const {
+real_t Vector2::distance_squared_to(const Vector2& p_vector2) const {
 
 	return (x-p_vector2.x)*(x-p_vector2.x) + (y-p_vector2.y)*(y-p_vector2.y);
 }
 
-float Vector2::angle_to(const Vector2& p_vector2) const  {
+real_t Vector2::angle_to(const Vector2& p_vector2) const  {
 
-	return Math::atan2( tangent().dot(p_vector2), dot(p_vector2) );
+	return Math::atan2( cross(p_vector2), dot(p_vector2) );
 }
 
-float Vector2::angle_to_point(const Vector2& p_vector2) const  {
+real_t Vector2::angle_to_point(const Vector2& p_vector2) const  {
 
-	return Math::atan2( x-p_vector2.x, y - p_vector2.y );
+	return Math::atan2( y - p_vector2.y, x-p_vector2.x );
 }
 
-float Vector2::dot(const Vector2& p_other) const {
+real_t Vector2::dot(const Vector2& p_other) const {
 
 	return x*p_other.x + y*p_other.y;
 }
 
-float Vector2::cross(const Vector2& p_other) const {
+real_t Vector2::cross(const Vector2& p_other) const {
 
 	return x*p_other.y - y*p_other.x;
 }
@@ -120,11 +120,11 @@ Vector2 Vector2::operator*(const Vector2 &p_v1) const {
 	return Vector2(x * p_v1.x, y * p_v1.y);
 };
 
-Vector2 Vector2::operator*(const float &rvalue) const {
+Vector2 Vector2::operator*(const real_t &rvalue) const {
 
 	return Vector2(x * rvalue, y * rvalue);
 };
-void Vector2::operator*=(const float &rvalue) {
+void Vector2::operator*=(const real_t &rvalue) {
 
 	x *= rvalue; y *= rvalue;
 };
@@ -134,12 +134,12 @@ Vector2 Vector2::operator/(const Vector2 &p_v1) const {
 	return Vector2(x / p_v1.x, y / p_v1.y);
 };
 
-Vector2 Vector2::operator/(const float &rvalue) const {
+Vector2 Vector2::operator/(const real_t &rvalue) const {
 
 	return Vector2(x / rvalue, y / rvalue);
 };
 
-void Vector2::operator/=(const float &rvalue) {
+void Vector2::operator/=(const real_t &rvalue) {
 
 	x /= rvalue; y /= rvalue;
 };
@@ -162,7 +162,7 @@ Vector2 Vector2::floor() const {
 	return Vector2( Math::floor(x), Math::floor(y) );
 }
 
-Vector2 Vector2::rotated(float p_by) const {
+Vector2 Vector2::rotated(real_t p_by) const {
 
 	Vector2 v;
 	v.set_rotation(angle()+p_by);
@@ -198,7 +198,7 @@ Vector2 Vector2::clamped(real_t p_len) const {
 	return v;
 }
 
-Vector2 Vector2::cubic_interpolate_soft(const Vector2& p_b,const Vector2& p_pre_a, const Vector2& p_post_b,float p_t) const {
+Vector2 Vector2::cubic_interpolate_soft(const Vector2& p_b,const Vector2& p_pre_a, const Vector2& p_post_b,real_t p_t) const {
 #if 0
 	k[0] = ((*this) (vi[0] + 1, vi[1], vi[2])) - ((*this) (vi[0],
 	vi[1],vi[2])); //fk = a0
@@ -219,13 +219,13 @@ Vector2 Vector2::cubic_interpolate_soft(const Vector2& p_b,const Vector2& p_pre_
 	//dk = (fk+1 - fk-1)*0.5
 	//Dk = (fk+1 - fk)
 
-	float dk =
+	real_t dk =
 #endif
 
 	return Vector2();
 }
 
-Vector2 Vector2::cubic_interpolate(const Vector2& p_b,const Vector2& p_pre_a, const Vector2& p_post_b,float p_t) const {
+Vector2 Vector2::cubic_interpolate(const Vector2& p_b,const Vector2& p_pre_a, const Vector2& p_post_b,real_t p_t) const {
 
 
 
@@ -234,9 +234,9 @@ Vector2 Vector2::cubic_interpolate(const Vector2& p_b,const Vector2& p_pre_a, co
 	Vector2 p2=p_b;
 	Vector2 p3=p_post_b;
 
-	float t = p_t;
-	float t2 = t * t;
-	float t3 = t2 * t;
+	real_t t = p_t;
+	real_t t2 = t * t;
+	real_t t3 = t2 * t;
 
 	Vector2 out;
 	out = 0.5f * ( ( p1 * 2.0f) +
@@ -246,8 +246,8 @@ Vector2 Vector2::cubic_interpolate(const Vector2& p_b,const Vector2& p_pre_a, co
 	return out;
 
 /*
-	float mu = p_t;
-	float mu2 = mu*mu;
+	real_t mu = p_t;
+	real_t mu2 = mu*mu;
 
 	Vector2 a0 = p_post_b - p_b - p_pre_a + *this;
 	Vector2 a1 = p_pre_a - *this - a0;
@@ -257,7 +257,7 @@ Vector2 Vector2::cubic_interpolate(const Vector2& p_b,const Vector2& p_pre_a, co
 	return ( a0*mu*mu2 + a1*mu2 + a2*mu + a3 );
 */
 	/*
-	float t = p_t;
+	real_t t = p_t;
 	real_t t2 = t*t;
 	real_t t3 = t2*t;
 
@@ -291,7 +291,7 @@ bool Rect2::intersects_segment(const Point2& p_from, const Point2& p_to, Point2*
 
 	real_t min=0,max=1;
 	int axis=0;
-	float sign=0;
+	real_t sign=0;
 
 	for(int i=0;i<2;i++) {
 		real_t seg_from=p_from[i];
@@ -299,7 +299,7 @@ bool Rect2::intersects_segment(const Point2& p_from, const Point2& p_to, Point2*
 		real_t box_begin=pos[i];
 		real_t box_end=box_begin+size[i];
 		real_t cmin,cmax;
-		float csign;
+		real_t csign;
 
 		if (seg_from < seg_to) {
 
@@ -409,7 +409,8 @@ bool Point2i::operator!=(const Point2i& p_vec2) const {
 }
 
 void Matrix32::invert() {
-
+	// FIXME: this function assumes the basis is a rotation matrix, with no scaling.
+	// Matrix32::affine_inverse can handle matrices with scaling, so GDScript should eventually use that.
 	SWAP(elements[0][1],elements[1][0]);
 	elements[2] = basis_xform(-elements[2]);
 }
@@ -424,9 +425,9 @@ Matrix32 Matrix32::inverse() const {
 
 void Matrix32::affine_invert() {
 
-	float det = basis_determinant();
+	real_t det = basis_determinant();
 	ERR_FAIL_COND(det==0);
-	float idet = 1.0 / det;
+	real_t idet = 1.0 / det;
 
 	SWAP( elements[0][0],elements[1][1] );
 	elements[0]*=Vector2(idet,-idet);
@@ -444,14 +445,16 @@ Matrix32 Matrix32::affine_inverse() const {
 }
 
 void Matrix32::rotate(real_t p_phi) {
-
-	Matrix32 rot(p_phi,Vector2());
-	*this *= rot;
+	*this = Matrix32(p_phi,Vector2()) * (*this);
 }
 
 real_t Matrix32::get_rotation() const {
-
-	return Math::atan2(elements[1].x,elements[1].y);
+	real_t det = basis_determinant();
+	Matrix32 m = orthonormalized();
+	if (det < 0) {
+		m.scale_basis(Size2(-1,-1));
+	}
+	return Math::atan2(m[0].y,m[0].x);
 }
 
 void Matrix32::set_rotation(real_t p_rot) {
@@ -459,9 +462,9 @@ void Matrix32::set_rotation(real_t p_rot) {
 	real_t cr = Math::cos(p_rot);
 	real_t sr = Math::sin(p_rot);
 	elements[0][0]=cr;
+	elements[0][1]=sr;
+	elements[1][0]=-sr;
 	elements[1][1]=cr;
-	elements[0][1]=-sr;
-	elements[1][0]=sr;
 }
 
 Matrix32::Matrix32(real_t p_rot, const Vector2& p_pos) {
@@ -469,27 +472,27 @@ Matrix32::Matrix32(real_t p_rot, const Vector2& p_pos) {
 	real_t cr = Math::cos(p_rot);
 	real_t sr = Math::sin(p_rot);
 	elements[0][0]=cr;
+	elements[0][1]=sr;
+	elements[1][0]=-sr;
 	elements[1][1]=cr;
-	elements[0][1]=-sr;
-	elements[1][0]=sr;
 	elements[2]=p_pos;
 }
 
 Size2 Matrix32::get_scale() const {
-
-	return Size2( elements[0].length(), elements[1].length() );
+	real_t det_sign = basis_determinant() > 0 ? 1 : -1;
+	return det_sign * Size2( elements[0].length(), elements[1].length() );
 }
 
 void Matrix32::scale(const Size2& p_scale) {
-
-	elements[0]*=p_scale;
-	elements[1]*=p_scale;
+	scale_basis(p_scale);
 	elements[2]*=p_scale;
 }
 void Matrix32::scale_basis(const Size2& p_scale) {
 
-	elements[0]*=p_scale;
-	elements[1]*=p_scale;
+	elements[0][0]*=p_scale.x;
+	elements[0][1]*=p_scale.y;
+	elements[1][0]*=p_scale.x;
+	elements[1][1]*=p_scale.y;
 
 }
 void Matrix32::translate( real_t p_tx, real_t p_ty) {
@@ -548,7 +551,7 @@ void Matrix32::operator*=(const Matrix32& p_transform) {
 
 	elements[2] = xform(p_transform.elements[2]);
 
-	float x0,x1,y0,y1;
+	real_t x0,x1,y0,y1;
 
 	x0 = tdotx(p_transform.elements[0]);
 	x1 = tdoty(p_transform.elements[0]);
@@ -601,7 +604,7 @@ Matrix32 Matrix32::translated(const Vector2& p_offset) const {
 
 }
 
-Matrix32 Matrix32::rotated(float p_phi) const {
+Matrix32 Matrix32::rotated(real_t p_phi) const {
 
 	Matrix32 copy=*this;
 	copy.rotate(p_phi);
@@ -609,12 +612,12 @@ Matrix32 Matrix32::rotated(float p_phi) const {
 
 }
 
-float Matrix32::basis_determinant() const {
+real_t Matrix32::basis_determinant() const {
 
 	return elements[0].x * elements[1].y - elements[0].y * elements[1].x;
 }
 
-Matrix32 Matrix32::interpolate_with(const Matrix32& p_transform, float p_c) const {
+Matrix32 Matrix32::interpolate_with(const Matrix32& p_transform, real_t p_c) const {
 
 	//extract parameters
 	Vector2 p1 = get_origin();
diff --git a/core/math/math_2d.h b/core/math/math_2d.h
index 2ec0dc39c5..adc23f01b1 100644
--- a/core/math/math_2d.h
+++ b/core/math/math_2d.h
@@ -65,35 +65,35 @@ enum VAlign {
 struct Vector2 {
 
 	union {
-		float x;
-		float width;
+		real_t x;
+		real_t width;
 	};
 	union {
-		float y;
-		float height;
+		real_t y;
+		real_t height;
 	};
 
 
-	_FORCE_INLINE_ float& operator[](int p_idx) {
+	_FORCE_INLINE_ real_t& operator[](int p_idx) {
 		return p_idx?y:x;
 	}
-	_FORCE_INLINE_ const float& operator[](int p_idx) const {
+	_FORCE_INLINE_ const real_t& operator[](int p_idx) const {
 		return p_idx?y:x;
 	}
 
 	void normalize();
 	Vector2 normalized() const;
 
-	float length() const;
-	float length_squared() const;
+	real_t length() const;
+	real_t length_squared() const;
 
-	float distance_to(const Vector2& p_vector2) const;
-	float distance_squared_to(const Vector2& p_vector2) const;
-	float angle_to(const Vector2& p_vector2) const;
-	float angle_to_point(const Vector2& p_vector2) const;
+	real_t distance_to(const Vector2& p_vector2) const;
+	real_t distance_squared_to(const Vector2& p_vector2) const;
+	real_t angle_to(const Vector2& p_vector2) const;
+	real_t angle_to_point(const Vector2& p_vector2) const;
 
-	float dot(const Vector2& p_other) const;
-	float cross(const Vector2& p_other) const;
+	real_t dot(const Vector2& p_other) const;
+	real_t cross(const Vector2& p_other) const;
 	Vector2 cross(real_t p_other) const;
 	Vector2 project(const Vector2& p_vec) const;
 
@@ -101,10 +101,10 @@ struct Vector2 {
 
 	Vector2 clamped(real_t p_len) const;
 
-	_FORCE_INLINE_ static Vector2 linear_interpolate(const Vector2& p_a, const Vector2& p_b,float p_t);
-	_FORCE_INLINE_ Vector2 linear_interpolate(const Vector2& p_b,float p_t) const;
-	Vector2 cubic_interpolate(const Vector2& p_b,const Vector2& p_pre_a, const Vector2& p_post_b,float p_t) const;
-	Vector2 cubic_interpolate_soft(const Vector2& p_b,const Vector2& p_pre_a, const Vector2& p_post_b,float p_t) const;
+	_FORCE_INLINE_ static Vector2 linear_interpolate(const Vector2& p_a, const Vector2& p_b,real_t p_t);
+	_FORCE_INLINE_ Vector2 linear_interpolate(const Vector2& p_b,real_t p_t) const;
+	Vector2 cubic_interpolate(const Vector2& p_b,const Vector2& p_pre_a, const Vector2& p_post_b,real_t p_t) const;
+	Vector2 cubic_interpolate_soft(const Vector2& p_b,const Vector2& p_pre_a, const Vector2& p_post_b,real_t p_t) const;
 
 	Vector2 slide(const Vector2& p_vec) const;
 	Vector2 reflect(const Vector2& p_vec) const;
@@ -115,15 +115,15 @@ struct Vector2 {
 	void operator-=(const Vector2& p_v);
 	Vector2 operator*(const Vector2 &p_v1) const;
 
-	Vector2 operator*(const float &rvalue) const;
-	void operator*=(const float &rvalue);
+	Vector2 operator*(const real_t &rvalue) const;
+	void operator*=(const real_t &rvalue);
 	void operator*=(const Vector2 &rvalue) { *this = *this * rvalue; }
 
 	Vector2 operator/(const Vector2 &p_v1) const;
 
-	Vector2 operator/(const float &rvalue) const;
+	Vector2 operator/(const real_t &rvalue) const;
 
-	void operator/=(const float &rvalue);
+	void operator/=(const real_t &rvalue);
 
 	Vector2 operator-() const;
 
@@ -135,10 +135,10 @@ struct Vector2 {
 
 	real_t angle() const;
 
-	void set_rotation(float p_radians) {
+	void set_rotation(real_t p_radians) {
 
-		x=Math::sin(p_radians);
-		y=Math::cos(p_radians);
+		x=Math::cos(p_radians);
+		y=Math::sin(p_radians);
 	}
 
 	_FORCE_INLINE_ Vector2 abs() const {
@@ -146,7 +146,7 @@ struct Vector2 {
 		return Vector2( Math::abs(x), Math::abs(y) );
 	}
 
-	Vector2 rotated(float p_by) const;
+	Vector2 rotated(real_t p_by) const;
 	Vector2 tangent() const {
 
 		return Vector2(y,-x);
@@ -154,12 +154,12 @@ struct Vector2 {
 
 	Vector2 floor() const;
 	Vector2 snapped(const Vector2& p_by) const;
-	float get_aspect() const { return width/height; }
+	real_t get_aspect() const { return width/height; }
 
 
 	operator String() const { return String::num(x)+", "+String::num(y); }
 
-	_FORCE_INLINE_ Vector2(float p_x,float p_y) { x=p_x; y=p_y; }
+	_FORCE_INLINE_ Vector2(real_t p_x,real_t p_y) { x=p_x; y=p_y; }
 	_FORCE_INLINE_ Vector2() { x=0; y=0; }
 };
 
@@ -169,12 +169,12 @@ _FORCE_INLINE_ Vector2 Vector2::plane_project(real_t p_d, const Vector2& p_vec)
 }
 
 
-_FORCE_INLINE_ Vector2 operator*(float p_scalar, const Vector2& p_vec)  {
+_FORCE_INLINE_ Vector2 operator*(real_t p_scalar, const Vector2& p_vec)  {
 
 	return p_vec*p_scalar;
 }
 
-Vector2 Vector2::linear_interpolate(const Vector2& p_b,float p_t) const {
+Vector2 Vector2::linear_interpolate(const Vector2& p_b,real_t p_t) const {
 
 	Vector2 res=*this;
 
@@ -185,7 +185,7 @@ Vector2 Vector2::linear_interpolate(const Vector2& p_b,float p_t) const {
 
 }
 
-Vector2 Vector2::linear_interpolate(const Vector2& p_a, const Vector2& p_b,float p_t)  {
+Vector2 Vector2::linear_interpolate(const Vector2& p_a, const Vector2& p_b,real_t p_t)  {
 
 	Vector2 res=p_a;
 
@@ -211,7 +211,7 @@ struct Rect2 {
 	const Vector2& get_size() const { return size; }
 	void set_size(const Vector2& p_size) { size=p_size; }
 
-	float get_area() const { return size.width*size.height; }
+	real_t get_area() const { return size.width*size.height; }
 
 	inline bool intersects(const Rect2& p_rect) const {
 		if ( pos.x >= (p_rect.pos.x + p_rect.size.width) )
@@ -226,9 +226,9 @@ struct Rect2 {
 		return true;
 	}
 
-	inline float distance_to(const Vector2& p_point) const {
+	inline real_t distance_to(const Vector2& p_point) const {
 
-		float dist = 1e20;
+		real_t dist = 1e20;
 
 		if (p_point.x < pos.x) {
 			dist=MIN(dist,pos.x-p_point.x);
@@ -359,7 +359,7 @@ struct Rect2 {
 	operator String() const { return String(pos)+", "+String(size); }
 
 	Rect2() {}
-	Rect2( float p_x, float p_y, float p_width, float p_height) { pos=Point2(p_x,p_y); size=Size2( p_width, p_height ); }
+	Rect2( real_t p_x, real_t p_y, real_t p_width, real_t p_height) { pos=Point2(p_x,p_y); size=Size2( p_width, p_height ); }
 	Rect2( const Point2& p_pos, const Size2& p_size ) { pos=p_pos; size=p_size; }
 };
 
@@ -407,7 +407,7 @@ struct Point2i {
 	bool operator==(const Point2i& p_vec2) const;
 	bool operator!=(const Point2i& p_vec2) const;
 
-	float get_aspect() const { return width/(float)height; }
+	real_t get_aspect() const { return width/(real_t)height; }
 
 	operator String() const { return String::num(x)+", "+String::num(y); }
 
@@ -552,11 +552,21 @@ struct Rect2i {
 
 
 struct Matrix32 {
+	// Warning #1: basis of Matrix32 is stored differently from Matrix3. In terms of elements array, the basis matrix looks like "on paper":
+	// M = (elements[0][0] elements[1][0])
+	//     (elements[0][1] elements[1][1])
+	// This is such that the columns, which can be interpreted as basis vectors of the coordinate system "painted" on the object, can be accessed as elements[i].
+	// Note that this is the opposite of the indices in mathematical texts, meaning: $M_{12}$ in a math book corresponds to elements[1][0] here.
+	// This requires additional care when working with explicit indices.
+ 	// See https://en.wikipedia.org/wiki/Row-_and_column-major_order for further reading.
+
+	// Warning #2: 2D be aware that unlike 3D code, 2D code uses a left-handed coordinate system: Y-axis points down,
+	// and angle is measure from +X to +Y in a clockwise-fashion.
 
 	Vector2 elements[3];
 
-	_FORCE_INLINE_ float tdotx(const Vector2& v) const { return elements[0][0] * v.x + elements[1][0] * v.y; }
-	_FORCE_INLINE_ float tdoty(const Vector2& v) const { return elements[0][1] * v.x + elements[1][1] * v.y; }
+	_FORCE_INLINE_ real_t tdotx(const Vector2& v) const { return elements[0][0] * v.x + elements[1][0] * v.y; }
+	_FORCE_INLINE_ real_t tdoty(const Vector2& v) const { return elements[0][1] * v.x + elements[1][1] * v.y; }
 
 	const Vector2& operator[](int p_idx) const { return elements[p_idx]; }
 	Vector2& operator[](int p_idx) { return elements[p_idx]; }
@@ -580,7 +590,7 @@ struct Matrix32 {
 	void translate( real_t p_tx, real_t p_ty);
 	void translate( const Vector2& p_translation );
 
-	float basis_determinant() const;
+	real_t basis_determinant() const;
 
 	Size2 get_scale() const;
 
@@ -590,7 +600,7 @@ struct Matrix32 {
 	Matrix32 scaled(const Size2& p_scale) const;
 	Matrix32 basis_scaled(const Size2& p_scale) const;
 	Matrix32 translated(const Vector2& p_offset) const;
-	Matrix32 rotated(float p_phi) const;
+	Matrix32 rotated(real_t p_phi) const;
 
 	Matrix32 untranslated() const;
 
@@ -603,7 +613,7 @@ struct Matrix32 {
 	void operator*=(const Matrix32& p_transform);
 	Matrix32 operator*(const Matrix32& p_transform) const;
 
-	Matrix32 interpolate_with(const Matrix32& p_transform, float p_c) const;
+	Matrix32 interpolate_with(const Matrix32& p_transform, real_t p_c) const;
 
 	_FORCE_INLINE_ Vector2 basis_xform(const Vector2& p_vec) const;
 	_FORCE_INLINE_ Vector2 basis_xform_inv(const Vector2& p_vec) const;
@@ -834,8 +844,8 @@ void Matrix32::set_rotation_and_scale(real_t p_rot,const Size2& p_scale) {
 
 	elements[0][0]=Math::cos(p_rot)*p_scale.x;
 	elements[1][1]=Math::cos(p_rot)*p_scale.y;
-	elements[0][1]=-Math::sin(p_rot)*p_scale.x;
-	elements[1][0]=Math::sin(p_rot)*p_scale.y;
+	elements[1][0]=-Math::sin(p_rot)*p_scale.y;
+	elements[0][1]=Math::sin(p_rot)*p_scale.x;
 
 }
 
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;
diff --git a/tools/editor/plugins/canvas_item_editor_plugin.cpp b/tools/editor/plugins/canvas_item_editor_plugin.cpp
index 5fa0d88ca5..df33651684 100644
--- a/tools/editor/plugins/canvas_item_editor_plugin.cpp
+++ b/tools/editor/plugins/canvas_item_editor_plugin.cpp
@@ -1601,10 +1601,8 @@ void CanvasItemEditor::_viewport_input_event(const InputEvent& p_event) {
 
 
 					if (node) {
-						Matrix32 rot;
-						rot.elements[1] = (dfrom - center).normalized();
-						rot.elements[0] = rot.elements[1].tangent();
-						node->set_rotation(snap_angle(rot.xform_inv(dto-center).angle() + node->get_rotation(), node->get_rotation()));
+						real_t angle = node->get_rotation();
+						node->set_rotation(snap_angle( angle + (dfrom - center).angle_to(dto-center), angle ));
 						display_rotate_to = dto;
 						display_rotate_from = center;
 						viewport->update();
@@ -1616,10 +1614,8 @@ void CanvasItemEditor::_viewport_input_event(const InputEvent& p_event) {
 
 
 					if (node) {
-						Matrix32 rot;
-						rot.elements[1] = (dfrom - center).normalized();
-						rot.elements[0] = rot.elements[1].tangent();
-						node->set_rotation(snap_angle(rot.xform_inv(dto-center).angle() + node->get_rotation(), node->get_rotation()));
+						real_t angle = node->get_rotation();
+						node->set_rotation(snap_angle( angle + (dfrom - center).angle_to(dto-center), angle ));
 						display_rotate_to = dto;
 						display_rotate_from = center;
 						viewport->update();