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.

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();

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;
 
 }
 

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);

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);

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;
 

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);\

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;

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();