155 lines
6.6 KiB
Diff
155 lines
6.6 KiB
Diff
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 <box3d/box3d.h>
|
|
+
|
|
+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<Box3DWeldJointImpl3D*>(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<Box3DWeldJointImpl3D*>(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) {
|