Merge pull request #13836 from AndreaCatania/scale

Fixed bullet scale on get_transform and joints

9 files changed, 100 insertions, 57 deletions

diff --git a/modules/bullet/collision_object_bullet.cpp b/modules/bullet/collision_object_bullet.cpp
index da3a4b73cf..88d4108f82 100644
--- a/modules/bullet/collision_object_bullet.cpp
+++ b/modules/bullet/collision_object_bullet.cpp
@@ -76,11 +76,17 @@ bool equal(real_t first, real_t second) {
 
 void CollisionObjectBullet::set_body_scale(const Vector3 &p_new_scale) {
 	if (!equal(p_new_scale[0], body_scale[0]) || !equal(p_new_scale[1], body_scale[1]) || !equal(p_new_scale[2], body_scale[2])) {
-		G_TO_B(p_new_scale, body_scale);
+		body_scale = p_new_scale;
 		on_body_scale_changed();
 	}
 }
 
+btVector3 CollisionObjectBullet::get_bt_body_scale() const {
+	btVector3 s;
+	G_TO_B(body_scale, s);
+	return s;
+}
+
 void CollisionObjectBullet::on_body_scale_changed() {
 }
 
@@ -160,6 +166,7 @@ void CollisionObjectBullet::set_transform(const Transform &p_global_transform) {
 Transform CollisionObjectBullet::get_transform() const {
 	Transform t;
 	B_TO_G(get_transform__bullet(), t);
+	t.basis.scale(body_scale);
 	return t;
 }
 
@@ -302,7 +309,7 @@ void RigidCollisionObjectBullet::on_shapes_changed() {
 		}
 	}
 
-	compoundShape->setLocalScaling(body_scale);
+	compoundShape->setLocalScaling(get_bt_body_scale());
 	compoundShape->recalculateLocalAabb();
 }
 
diff --git a/modules/bullet/collision_object_bullet.h b/modules/bullet/collision_object_bullet.h
index 51e48909e4..7d4659b64e 100644
--- a/modules/bullet/collision_object_bullet.h
+++ b/modules/bullet/collision_object_bullet.h
@@ -114,7 +114,7 @@ protected:
 	bool m_isStatic;
 	bool ray_pickable;
 	btCollisionObject *bt_collision_object;
-	btVector3 body_scale;
+	Vector3 body_scale;
 	SpaceBullet *space;
 
 	VSet<RID> exceptions;
@@ -146,6 +146,8 @@ public:
 	_FORCE_INLINE_ bool is_ray_pickable() const { return ray_pickable; }
 
 	void set_body_scale(const Vector3 &p_new_scale);
+	const Vector3 &get_body_scale() const { return body_scale; }
+	btVector3 get_bt_body_scale() const;
 	virtual void on_body_scale_changed();
 
 	void add_collision_exception(const CollisionObjectBullet *p_ignoreCollisionObject);
diff --git a/modules/bullet/cone_twist_joint_bullet.cpp b/modules/bullet/cone_twist_joint_bullet.cpp
index 7ae5e79645..738835b910 100644
--- a/modules/bullet/cone_twist_joint_bullet.cpp
+++ b/modules/bullet/cone_twist_joint_bullet.cpp
@@ -37,11 +37,21 @@
 
 ConeTwistJointBullet::ConeTwistJointBullet(RigidBodyBullet *rbA, RigidBodyBullet *rbB, const Transform &rbAFrame, const Transform &rbBFrame) :
 		JointBullet() {
+
+	Transform scaled_AFrame(rbAFrame.scaled(rbA->get_body_scale()));
+	scaled_AFrame.basis.rotref_posscale_decomposition(scaled_AFrame.basis);
+
 	btTransform btFrameA;
-	G_TO_B(rbAFrame, btFrameA);
+	G_TO_B(scaled_AFrame, btFrameA);
+
 	if (rbB) {
+
+		Transform scaled_BFrame(rbBFrame.scaled(rbB->get_body_scale()));
+		scaled_BFrame.basis.rotref_posscale_decomposition(scaled_BFrame.basis);
+
 		btTransform btFrameB;
-		G_TO_B(rbBFrame, btFrameB);
+		G_TO_B(scaled_BFrame, btFrameB);
+
 		coneConstraint = bulletnew(btConeTwistConstraint(*rbA->get_bt_rigid_body(), *rbB->get_bt_rigid_body(), btFrameA, btFrameB));
 	} else {
 		coneConstraint = bulletnew(btConeTwistConstraint(*rbA->get_bt_rigid_body(), btFrameA));
diff --git a/modules/bullet/generic_6dof_joint_bullet.cpp b/modules/bullet/generic_6dof_joint_bullet.cpp
index 28928bd861..da09d4e12f 100644
--- a/modules/bullet/generic_6dof_joint_bullet.cpp
+++ b/modules/bullet/generic_6dof_joint_bullet.cpp
@@ -38,12 +38,20 @@
 Generic6DOFJointBullet::Generic6DOFJointBullet(RigidBodyBullet *rbA, RigidBodyBullet *rbB, const Transform &frameInA, const Transform &frameInB, bool useLinearReferenceFrameA) :
 		JointBullet() {
 
+	Transform scaled_AFrame(frameInA.scaled(rbA->get_body_scale()));
+
+	scaled_AFrame.basis.rotref_posscale_decomposition(scaled_AFrame.basis);
+
 	btTransform btFrameA;
-	G_TO_B(frameInA, btFrameA);
+	G_TO_B(scaled_AFrame, btFrameA);
 
 	if (rbB) {
+		Transform scaled_BFrame(frameInB.scaled(rbB->get_body_scale()));
+
+		scaled_BFrame.basis.rotref_posscale_decomposition(scaled_BFrame.basis);
+
 		btTransform btFrameB;
-		G_TO_B(frameInB, btFrameB);
+		G_TO_B(scaled_BFrame, btFrameB);
 
 		sixDOFConstraint = bulletnew(btGeneric6DofConstraint(*rbA->get_bt_rigid_body(), *rbB->get_bt_rigid_body(), btFrameA, btFrameB, useLinearReferenceFrameA));
 	} else {
@@ -109,10 +117,12 @@ void Generic6DOFJointBullet::set_param(Vector3::Axis p_axis, PhysicsServer::G6DO
 	ERR_FAIL_INDEX(p_axis, 3);
 	switch (p_param) {
 		case PhysicsServer::G6DOF_JOINT_LINEAR_LOWER_LIMIT:
-			sixDOFConstraint->getTranslationalLimitMotor()->m_lowerLimit[p_axis] = p_value;
+			limits_lower[0][p_axis] = p_value;
+			set_flag(p_axis, PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT, flags[p_axis][p_param]); // Reload bullet parameter
 			break;
 		case PhysicsServer::G6DOF_JOINT_LINEAR_UPPER_LIMIT:
-			sixDOFConstraint->getTranslationalLimitMotor()->m_upperLimit[p_axis] = p_value;
+			limits_upper[0][p_axis] = p_value;
+			set_flag(p_axis, PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT, flags[p_axis][p_param]); // Reload bullet parameter
 			break;
 		case PhysicsServer::G6DOF_JOINT_LINEAR_LIMIT_SOFTNESS:
 			sixDOFConstraint->getTranslationalLimitMotor()->m_limitSoftness = p_value;
@@ -124,10 +134,12 @@ void Generic6DOFJointBullet::set_param(Vector3::Axis p_axis, PhysicsServer::G6DO
 			sixDOFConstraint->getTranslationalLimitMotor()->m_damping = p_value;
 			break;
 		case PhysicsServer::G6DOF_JOINT_ANGULAR_LOWER_LIMIT:
-			sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_loLimit = p_value;
+			limits_lower[1][p_axis] = p_value;
+			set_flag(p_axis, PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT, flags[p_axis][p_param]); // Reload bullet parameter
 			break;
 		case PhysicsServer::G6DOF_JOINT_ANGULAR_UPPER_LIMIT:
-			sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_hiLimit = p_value;
+			limits_upper[1][p_axis] = p_value;
+			set_flag(p_axis, PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT, flags[p_axis][p_param]); // Reload bullet parameter
 			break;
 		case PhysicsServer::G6DOF_JOINT_ANGULAR_LIMIT_SOFTNESS:
 			sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_limitSoftness = p_value;
@@ -159,9 +171,9 @@ real_t Generic6DOFJointBullet::get_param(Vector3::Axis p_axis, PhysicsServer::G6
 	ERR_FAIL_INDEX_V(p_axis, 3, 0.);
 	switch (p_param) {
 		case PhysicsServer::G6DOF_JOINT_LINEAR_LOWER_LIMIT:
-			return sixDOFConstraint->getTranslationalLimitMotor()->m_lowerLimit[p_axis];
+			return limits_lower[0][p_axis];
 		case PhysicsServer::G6DOF_JOINT_LINEAR_UPPER_LIMIT:
-			return sixDOFConstraint->getTranslationalLimitMotor()->m_upperLimit[p_axis];
+			return limits_upper[0][p_axis];
 		case PhysicsServer::G6DOF_JOINT_LINEAR_LIMIT_SOFTNESS:
 			return sixDOFConstraint->getTranslationalLimitMotor()->m_limitSoftness;
 		case PhysicsServer::G6DOF_JOINT_LINEAR_RESTITUTION:
@@ -169,9 +181,9 @@ real_t Generic6DOFJointBullet::get_param(Vector3::Axis p_axis, PhysicsServer::G6
 		case PhysicsServer::G6DOF_JOINT_LINEAR_DAMPING:
 			return sixDOFConstraint->getTranslationalLimitMotor()->m_damping;
 		case PhysicsServer::G6DOF_JOINT_ANGULAR_LOWER_LIMIT:
-			return sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_loLimit;
+			return limits_lower[1][p_axis];
 		case PhysicsServer::G6DOF_JOINT_ANGULAR_UPPER_LIMIT:
-			return sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_hiLimit;
+			return limits_upper[1][p_axis];
 		case PhysicsServer::G6DOF_JOINT_ANGULAR_LIMIT_SOFTNESS:
 			return sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_limitSoftness;
 		case PhysicsServer::G6DOF_JOINT_ANGULAR_DAMPING:
@@ -194,48 +206,35 @@ real_t Generic6DOFJointBullet::get_param(Vector3::Axis p_axis, PhysicsServer::G6
 
 void Generic6DOFJointBullet::set_flag(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisFlag p_flag, bool p_value) {
 	ERR_FAIL_INDEX(p_axis, 3);
+
+	flags[p_axis][p_flag] = p_value;
+
 	switch (p_flag) {
 		case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT:
-			if (p_value) {
-				if (!get_flag(p_axis, p_flag)) // avoid overwrite, if limited
-					sixDOFConstraint->setLimit(p_axis, 0, 0); // Limited
+			if (flags[p_axis][p_flag]) {
+				sixDOFConstraint->setLimit(p_axis, limits_lower[0][p_axis], limits_upper[0][p_axis]);
 			} else {
-				if (get_flag(p_axis, p_flag)) // avoid overwrite, if free
-					sixDOFConstraint->setLimit(p_axis, 0, -1); // Free
+				sixDOFConstraint->setLimit(p_axis, 0, -1); // Free
 			}
 			break;
-		case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT: {
-			int angularAxis = 3 + p_axis;
-			if (p_value) {
-				if (!get_flag(p_axis, p_flag)) // avoid overwrite, if Limited
-					sixDOFConstraint->setLimit(angularAxis, 0, 0); // Limited
+		case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT:
+			if (flags[p_axis][p_flag]) {
+				sixDOFConstraint->setLimit(p_axis + 3, limits_lower[1][p_axis], limits_upper[1][p_axis]);
 			} else {
-				if (get_flag(p_axis, p_flag)) // avoid overwrite, if free
-					sixDOFConstraint->setLimit(angularAxis, 0, -1); // Free
+				sixDOFConstraint->setLimit(p_axis + 3, 0, -1); // Free
 			}
 			break;
-		}
 		case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_MOTOR:
-			//sixDOFConstraint->getTranslationalLimitMotor()->m_enableMotor[p_axis] = p_value;
-			sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_enableMotor = p_value;
+			sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_enableMotor = flags[p_axis][p_flag];
 			break;
 		default:
 			WARN_PRINT("This flag is not supported by Bullet engine");
+			return;
 	}
 }
 
 bool Generic6DOFJointBullet::get_flag(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisFlag p_flag) const {
 	ERR_FAIL_INDEX_V(p_axis, 3, false);
-	switch (p_flag) {
-		case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT:
-			return sixDOFConstraint->getTranslationalLimitMotor()->isLimited(p_axis);
-		case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT:
-			return sixDOFConstraint->getRotationalLimitMotor(p_axis)->isLimited();
-		case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_MOTOR:
-			return //sixDOFConstraint->getTranslationalLimitMotor()->m_enableMotor[p_axis] &&
-					sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_enableMotor;
-		default:
-			WARN_PRINT("This flag is not supported by Bullet engine");
-			return false;
-	}
+
+	return flags[p_axis][p_flag];
 }
diff --git a/modules/bullet/generic_6dof_joint_bullet.h b/modules/bullet/generic_6dof_joint_bullet.h
index 0d47b823de..ba0ae08800 100644
--- a/modules/bullet/generic_6dof_joint_bullet.h
+++ b/modules/bullet/generic_6dof_joint_bullet.h
@@ -39,6 +39,11 @@ class RigidBodyBullet;
 class Generic6DOFJointBullet : public JointBullet {
 	class btGeneric6DofConstraint *sixDOFConstraint;
 
+	// First is linear second is angular
+	Vector3 limits_lower[2];
+	Vector3 limits_upper[2];
+	bool flags[3][PhysicsServer::G6DOF_JOINT_FLAG_MAX];
+
 public:
 	Generic6DOFJointBullet(RigidBodyBullet *rbA, RigidBodyBullet *rbB, const Transform &frameInA, const Transform &frameInB, bool useLinearReferenceFrameA);
 
diff --git a/modules/bullet/hinge_joint_bullet.cpp b/modules/bullet/hinge_joint_bullet.cpp
index d3288807b3..ee0d6707d6 100644
--- a/modules/bullet/hinge_joint_bullet.cpp
+++ b/modules/bullet/hinge_joint_bullet.cpp
@@ -37,12 +37,20 @@
 
 HingeJointBullet::HingeJointBullet(RigidBodyBullet *rbA, RigidBodyBullet *rbB, const Transform &frameA, const Transform &frameB) :
 		JointBullet() {
+
+	Transform scaled_AFrame(frameA.scaled(rbA->get_body_scale()));
+	scaled_AFrame.basis.rotref_posscale_decomposition(scaled_AFrame.basis);
+
 	btTransform btFrameA;
-	G_TO_B(frameA, btFrameA);
+	G_TO_B(scaled_AFrame, btFrameA);
 
 	if (rbB) {
+
+		Transform scaled_BFrame(frameB.scaled(rbB->get_body_scale()));
+		scaled_BFrame.basis.rotref_posscale_decomposition(scaled_BFrame.basis);
+
 		btTransform btFrameB;
-		G_TO_B(frameB, btFrameB);
+		G_TO_B(scaled_BFrame, btFrameB);
 
 		hingeConstraint = bulletnew(btHingeConstraint(*rbA->get_bt_rigid_body(), *rbB->get_bt_rigid_body(), btFrameA, btFrameB));
 	} else {
@@ -58,14 +66,14 @@ HingeJointBullet::HingeJointBullet(RigidBodyBullet *rbA, RigidBodyBullet *rbB, c
 
 	btVector3 btPivotA;
 	btVector3 btAxisA;
-	G_TO_B(pivotInA, btPivotA);
-	G_TO_B(axisInA, btAxisA);
+	G_TO_B(pivotInA * rbA->get_body_scale(), btPivotA);
+	G_TO_B(axisInA * rbA->get_body_scale(), btAxisA);
 
 	if (rbB) {
 		btVector3 btPivotB;
 		btVector3 btAxisB;
-		G_TO_B(pivotInB, btPivotB);
-		G_TO_B(axisInB, btAxisB);
+		G_TO_B(pivotInB * rbB->get_body_scale(), btPivotB);
+		G_TO_B(axisInB * rbB->get_body_scale(), btAxisB);
 
 		hingeConstraint = bulletnew(btHingeConstraint(*rbA->get_bt_rigid_body(), *rbB->get_bt_rigid_body(), btPivotA, btPivotB, btAxisA, btAxisB));
 	} else {
diff --git a/modules/bullet/pin_joint_bullet.cpp b/modules/bullet/pin_joint_bullet.cpp
index 8c74fcbc94..665e825967 100644
--- a/modules/bullet/pin_joint_bullet.cpp
+++ b/modules/bullet/pin_joint_bullet.cpp
@@ -40,8 +40,8 @@ PinJointBullet::PinJointBullet(RigidBodyBullet *p_body_a, const Vector3 &p_pos_a
 
 		btVector3 btPivotA;
 		btVector3 btPivotB;
-		G_TO_B(p_pos_a, btPivotA);
-		G_TO_B(p_pos_b, btPivotB);
+		G_TO_B(p_pos_a * p_body_a->get_body_scale(), btPivotA);
+		G_TO_B(p_pos_b * p_body_b->get_body_scale(), btPivotB);
 		p2pConstraint = bulletnew(btPoint2PointConstraint(*p_body_a->get_bt_rigid_body(),
 				*p_body_b->get_bt_rigid_body(),
 				btPivotA,
diff --git a/modules/bullet/rigid_body_bullet.cpp b/modules/bullet/rigid_body_bullet.cpp
index dd2e17c110..669b2c3f0c 100644
--- a/modules/bullet/rigid_body_bullet.cpp
+++ b/modules/bullet/rigid_body_bullet.cpp
@@ -198,6 +198,8 @@ void RigidBodyBullet::KinematicUtilities::copyAllOwnerShapes() {
 
 	const CollisionObjectBullet::ShapeWrapper *shape_wrapper;
 
+	btVector3 owner_body_scale(owner->get_bt_body_scale());
+
 	for (int i = shapes_count - 1; 0 <= i; --i) {
 		shape_wrapper = &shapes_wrappers[i];
 		if (!shape_wrapper->active) {
@@ -210,28 +212,29 @@ void RigidBodyBullet::KinematicUtilities::copyAllOwnerShapes() {
 		switch (shape_wrapper->shape->get_type()) {
 			case PhysicsServer::SHAPE_SPHERE: {
 				SphereShapeBullet *sphere = static_cast<SphereShapeBullet *>(shape_wrapper->shape);
-				kin_shape_ref = ShapeBullet::create_shape_sphere(sphere->get_radius() * owner->body_scale[0] + safe_margin);
+				kin_shape_ref = ShapeBullet::create_shape_sphere(sphere->get_radius() * owner_body_scale[0] + safe_margin);
 				break;
 			}
 			case PhysicsServer::SHAPE_BOX: {
 				BoxShapeBullet *box = static_cast<BoxShapeBullet *>(shape_wrapper->shape);
-				kin_shape_ref = ShapeBullet::create_shape_box((box->get_half_extents() * owner->body_scale) + btVector3(safe_margin, safe_margin, safe_margin));
+				kin_shape_ref = ShapeBullet::create_shape_box((box->get_half_extents() * owner_body_scale) + btVector3(safe_margin, safe_margin, safe_margin));
 				break;
 			}
 			case PhysicsServer::SHAPE_CAPSULE: {
 				CapsuleShapeBullet *capsule = static_cast<CapsuleShapeBullet *>(shape_wrapper->shape);
-				kin_shape_ref = ShapeBullet::create_shape_capsule(capsule->get_radius() * owner->body_scale[0] + safe_margin, capsule->get_height() * owner->body_scale[1] + safe_margin);
+
+				kin_shape_ref = ShapeBullet::create_shape_capsule(capsule->get_radius() * owner_body_scale[0] + safe_margin, capsule->get_height() * owner_body_scale[1] + safe_margin);
 				break;
 			}
 			case PhysicsServer::SHAPE_CONVEX_POLYGON: {
 				ConvexPolygonShapeBullet *godot_convex = static_cast<ConvexPolygonShapeBullet *>(shape_wrapper->shape);
 				kin_shape_ref = ShapeBullet::create_shape_convex(godot_convex->vertices);
-				kin_shape_ref->setLocalScaling(owner->body_scale + btVector3(safe_margin, safe_margin, safe_margin));
+				kin_shape_ref->setLocalScaling(owner_body_scale + btVector3(safe_margin, safe_margin, safe_margin));
 				break;
 			}
 			case PhysicsServer::SHAPE_RAY: {
 				RayShapeBullet *godot_ray = static_cast<RayShapeBullet *>(shape_wrapper->shape);
-				kin_shape_ref = ShapeBullet::create_shape_ray(godot_ray->length * owner->body_scale[1] + safe_margin);
+				kin_shape_ref = ShapeBullet::create_shape_ray(godot_ray->length * owner_body_scale[1] + safe_margin);
 				break;
 			}
 			default:
diff --git a/modules/bullet/slider_joint_bullet.cpp b/modules/bullet/slider_joint_bullet.cpp
index f1d60679ec..cfcd0b57f6 100644
--- a/modules/bullet/slider_joint_bullet.cpp
+++ b/modules/bullet/slider_joint_bullet.cpp
@@ -37,11 +37,20 @@
 
 SliderJointBullet::SliderJointBullet(RigidBodyBullet *rbA, RigidBodyBullet *rbB, const Transform &frameInA, const Transform &frameInB) :
 		JointBullet() {
+
+	Transform scaled_AFrame(frameInA.scaled(rbA->get_body_scale()));
+	scaled_AFrame.basis.rotref_posscale_decomposition(scaled_AFrame.basis);
+
 	btTransform btFrameA;
-	G_TO_B(frameInA, btFrameA);
+	G_TO_B(scaled_AFrame, btFrameA);
+
 	if (rbB) {
+
+		Transform scaled_BFrame(frameInB.scaled(rbB->get_body_scale()));
+		scaled_BFrame.basis.rotref_posscale_decomposition(scaled_BFrame.basis);
+
 		btTransform btFrameB;
-		G_TO_B(frameInB, btFrameB);
+		G_TO_B(scaled_BFrame, btFrameB);
 		sliderConstraint = bulletnew(btSliderConstraint(*rbA->get_bt_rigid_body(), *rbB->get_bt_rigid_body(), btFrameA, btFrameB, true));
 
 	} else {