diff --git a/modules/bullet/cone_twist_joint_bullet.cpp b/modules/bullet/cone_twist_joint_bullet.cpp index 7ae5e796451..738835b910a 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 28928bd8616..2b17738c317 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 { diff --git a/modules/bullet/hinge_joint_bullet.cpp b/modules/bullet/hinge_joint_bullet.cpp index d3288807b35..ee0d6707d63 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 8c74fcbc947..665e8259679 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/slider_joint_bullet.cpp b/modules/bullet/slider_joint_bullet.cpp index f1d60679ecb..cfcd0b57f64 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 {