diff --git a/src/joints/box3d_weld_joint_impl_3d.cpp b/src/joints/box3d_weld_joint_impl_3d.cpp new file mode 100644 index 0000000..ed4c687 --- /dev/null +++ b/src/joints/box3d_weld_joint_impl_3d.cpp @@ -0,0 +1,64 @@ +#include "box3d_weld_joint_impl_3d.hpp" + +#include "../misc/type_conversions.hpp" + +#include + +b3JointId Box3DWeldJointImpl3D::_create_joint_id(b3WorldId p_world_id, b3BodyId p_body_a, b3BodyId p_body_b, b3Transform p_local_frame_a, b3Transform p_local_frame_b) { + b3WeldJointDef def = b3DefaultWeldJointDef(); + def.base.bodyIdA = p_body_a; + def.base.bodyIdB = p_body_b; + def.base.localFrameA = p_local_frame_a; + + // The weld enforces relQ == identity against the two local frames, so frame B's rotation + // is pre-corrected with the bodies' CURRENT relative rotation: the joint then locks the + // relative pose at creation time, no matter how the joint node was oriented. + const b3Quat quat_a = b3Body_GetRotation(p_body_a); + const b3Quat quat_b = b3Body_GetRotation(p_body_b); + const b3Quat world_frame_a = b3MulQuat(quat_a, p_local_frame_a.q); + def.base.localFrameB.q = b3InvMulQuat(quat_b, world_frame_a); + def.base.localFrameB.p = p_local_frame_b.p; + + // hertz 0 = maximum stiffness (Box3D falls back to its engine-clamped world constraint + // softness); bias > 0 softens the weld: hertz = bias * 30, mirroring the pin joint. + def.linearHertz = (float)bias * 30.0f; + def.angularHertz = (float)bias * 30.0f; + def.linearDampingRatio = 1.0f; + def.angularDampingRatio = 1.0f; + return b3CreateWeldJoint(p_world_id, &def); +} + +real_t Box3DWeldJointImpl3D::get_param(PhysicsServer3D::ConeTwistJointParam p_param) const { + switch (p_param) { + case PhysicsServer3D::CONE_TWIST_JOINT_BIAS: + return bias; + default: + return 0.0; + } +} + +void Box3DWeldJointImpl3D::set_param(PhysicsServer3D::ConeTwistJointParam p_param, real_t p_value) { + switch (p_param) { + case PhysicsServer3D::CONE_TWIST_JOINT_BIAS: + bias = p_value; + if (has_joint_id()) { + b3WeldJoint_SetLinearHertz(get_joint_id(), (float)bias * 30.0f); + b3WeldJoint_SetAngularHertz(get_joint_id(), (float)bias * 30.0f); + } + break; + case PhysicsServer3D::CONE_TWIST_JOINT_SWING_SPAN: + case PhysicsServer3D::CONE_TWIST_JOINT_TWIST_SPAN: + if (p_value > 0.0) { + WARN_PRINT_ONCE("Box3D: ConeTwistJoint3D's swing/twist spans have no Box3D equivalent (the weld joint has no limits) and are treated as fully rigid."); + } + break; + case PhysicsServer3D::CONE_TWIST_JOINT_SOFTNESS: + WARN_PRINT_ONCE("Box3D: ConeTwistJoint3D's SOFTNESS parameter has no Box3D equivalent and is ignored; use BIAS to soften the weld."); + break; + case PhysicsServer3D::CONE_TWIST_JOINT_RELAXATION: + WARN_PRINT_ONCE("Box3D: ConeTwistJoint3D's RELAXATION parameter has no Box3D equivalent and is ignored."); + break; + default: + break; + } +} diff --git a/src/joints/box3d_weld_joint_impl_3d.hpp b/src/joints/box3d_weld_joint_impl_3d.hpp new file mode 100644 index 0000000..408fd3b --- /dev/null +++ b/src/joints/box3d_weld_joint_impl_3d.hpp @@ -0,0 +1,30 @@ +#pragma once + +#include "box3d_joint_impl_3d.hpp" + +// CONE_TWIST_JOINT mapped to Box3D's weld joint: a solver-owned rigid weld that locks the +// two bodies at the relative pose captured when the joint is created. Godot's +// ConeTwistJoint3D with swing_span 0 + twist_span 0 is therefore a rigid grab weld; nonzero +// spans have no Box3D equivalent (the weld has no limits) and are warned + treated as rigid. +class Box3DWeldJointImpl3D final : public Box3DJointImpl3D { +public: + Box3DWeldJointImpl3D( + Box3DBodyImpl3D* p_body_a, + Box3DBodyImpl3D* p_body_b, + const Transform3D& p_local_frame_a, + const Transform3D& p_local_frame_b) : + Box3DJointImpl3D(p_body_a, p_body_b, p_local_frame_a, p_local_frame_b) { + } + + PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_CONE_TWIST; } + + real_t get_param(PhysicsServer3D::ConeTwistJointParam p_param) const; + + void set_param(PhysicsServer3D::ConeTwistJointParam p_param, real_t p_value); + +protected: + b3JointId _create_joint_id(b3WorldId p_world_id, b3BodyId p_body_a, b3BodyId p_body_b, b3Transform p_local_frame_a, b3Transform p_local_frame_b) override; + +private: + real_t bias = 0.0; +}; diff --git a/src/servers/box3d_physics_server_3d.cpp b/src/servers/box3d_physics_server_3d.cpp index d7de27e..01565a2 100644 --- a/src/servers/box3d_physics_server_3d.cpp +++ b/src/servers/box3d_physics_server_3d.cpp @@ -5,6 +5,7 @@ #include "../joints/box3d_filter_joint_impl_3d.hpp" #include "../joints/box3d_pin_joint_impl_3d.hpp" #include "../joints/box3d_slider_joint_impl_3d.hpp" +#include "../joints/box3d_weld_joint_impl_3d.hpp" #include "../misc/type_conversions.hpp" #include "../objects/box3d_area_impl_3d.hpp" #include "../objects/box3d_body_impl_3d.hpp" @@ -1093,15 +1094,33 @@ double Box3DPhysicsServer3D::_slider_joint_get_param(const RID& p_joint, Physics return joint->get_param(p_param); } +// Box3D's weld joint has no cone/twist limits: a ConeTwistJoint3D with swing_span 0 + +// twist_span 0 is exactly a rigid weld at the captured relative pose, and nonzero spans are +// warned + treated as fully rigid (see Box3DWeldJointImpl3D). void Box3DPhysicsServer3D::_joint_make_cone_twist(const RID& p_joint, const RID& p_body_a, const Transform3D& p_local_ref_a, const RID& p_body_b, const Transform3D& p_local_ref_b) { - ERR_FAIL_MSG("Box3D: ConeTwistJoint3D is not supported in this version of the Box3D extension."); + _joint_clear(p_joint); + + Box3DBodyImpl3D* body_a = body_owner.get_or_null(p_body_a); + Box3DBodyImpl3D* body_b = body_owner.get_or_null(p_body_b); + ERR_FAIL_NULL(body_a); + ERR_FAIL_NULL(body_b); + + auto* joint = memnew(Box3DWeldJointImpl3D(body_a, body_b, p_local_ref_a, p_local_ref_b)); + joint->set_rid(p_joint); + joint_owner.replace(p_joint, joint); + joint->rebuild(); } void Box3DPhysicsServer3D::_cone_twist_joint_set_param(const RID& p_joint, PhysicsServer3D::ConeTwistJointParam p_param, double p_value) { + auto* joint = dynamic_cast(joint_owner.get_or_null(p_joint)); + ERR_FAIL_NULL(joint); + joint->set_param(p_param, p_value); } double Box3DPhysicsServer3D::_cone_twist_joint_get_param(const RID& p_joint, PhysicsServer3D::ConeTwistJointParam p_param) const { - return 0.0; + auto* joint = dynamic_cast(joint_owner.get_or_null(p_joint)); + ERR_FAIL_NULL_V(joint, 0.0); + return joint->get_param(p_param); } void Box3DPhysicsServer3D::_joint_make_generic_6dof(const RID& p_joint, const RID& p_body_a, const Transform3D& p_local_ref_a, const RID& p_body_b, const Transform3D& p_local_ref_b) {