From b0777e85b418ee093364906b1f025ef4eaec8881 Mon Sep 17 00:00:00 2001 From: Foxyz Date: Fri, 2 Oct 2026 05:09:40 +0300 Subject: [PATCH] Expose Box3D joints through Godot's Generic6DOFJoint3D Configuration-mapped: all linear axes locked -> b3SphericalJoint (joint X = twist axis, symmetric swing cone from angular Y/Z limits, rotation spring to the angular equilibrium, torque-capped velocity motor: per-axis target velocity in joint frame A, force limit = max torque); everything locked -> b3WeldJoint; any linear axis free -> b3MotorJoint (springs pull frame B onto frame A + equilibrium offset, motor force limits cap force and torque). Spring stiffness/damping are Box3D hertz/damping ratio. Flags and limits rebuild the joint; motor velocities, springs and equilibrium points update live. Co-Authored-By: Claude Opus 5.5 --- .../box3d_generic_6dof_joint_impl_3d.cpp | 289 ++++++++++++++++++ .../box3d_generic_6dof_joint_impl_3d.hpp | 72 +++++ src/servers/box3d_physics_server_3d.cpp | 27 +- 3 files changed, 385 insertions(+), 3 deletions(-) create mode 100644 src/joints/box3d_generic_6dof_joint_impl_3d.cpp create mode 100644 src/joints/box3d_generic_6dof_joint_impl_3d.hpp diff --git a/src/joints/box3d_generic_6dof_joint_impl_3d.cpp b/src/joints/box3d_generic_6dof_joint_impl_3d.cpp new file mode 100644 index 0000000..8de5f8f --- /dev/null +++ b/src/joints/box3d_generic_6dof_joint_impl_3d.cpp @@ -0,0 +1,289 @@ +#include "box3d_generic_6dof_joint_impl_3d.hpp" + +#include "../misc/type_conversions.hpp" +#include "../objects/box3d_body_impl_3d.hpp" + +#include + +#include + +namespace { + +// Box3D's spherical joint measures twist about frame z and centres the swing cone on frame A's +// z; Godot's joint convention twists about X. Post-rotating both frames by +90° about Y maps +// local z onto local X. +const Quaternion TWIST_REMAP(Vector3(0, 1, 0), Math_PI * 0.5); + +b3Transform remap_twist(const b3Transform& p_frame) { + b3Transform out = p_frame; + out.q = godot_to_b3(b3_to_godot(p_frame.q) * TWIST_REMAP); + return out; +} + +} // namespace + +Box3DGeneric6DOFJointImpl3D::Box3DGeneric6DOFJointImpl3D( + 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) { + // Godot's defaults: every axis limited to [0, 0], i.e. fully locked. + for (AxisState& axis : axes) { + axis.flag[PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT] = true; + axis.flag[PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT] = true; + } + mode = _classify(); +} + +bool Box3DGeneric6DOFJointImpl3D::_linear_locked(int p_axis) const { + const AxisState& a = axes[p_axis]; + return a.flag[PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT] && + a.param[PhysicsServer3D::G6DOF_JOINT_LINEAR_LOWER_LIMIT] >= a.param[PhysicsServer3D::G6DOF_JOINT_LINEAR_UPPER_LIMIT] && + !a.flag[PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_SPRING] && + !a.flag[PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_MOTOR]; +} + +bool Box3DGeneric6DOFJointImpl3D::_angular_locked(int p_axis) const { + const AxisState& a = axes[p_axis]; + return a.flag[PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT] && + a.param[PhysicsServer3D::G6DOF_JOINT_ANGULAR_LOWER_LIMIT] >= a.param[PhysicsServer3D::G6DOF_JOINT_ANGULAR_UPPER_LIMIT]; +} + +bool Box3DGeneric6DOFJointImpl3D::_any_flag(Flag p_flag) const { + return axes[0].flag[p_flag] || axes[1].flag[p_flag] || axes[2].flag[p_flag]; +} + +double Box3DGeneric6DOFJointImpl3D::_max_param(Param p_param, Flag p_gate) const { + double result = 0.0; + for (const AxisState& a : axes) { + if (a.flag[p_gate]) { + result = MAX(result, a.param[p_param]); + } + } + return result; +} + +Box3DGeneric6DOFJointImpl3D::Mode Box3DGeneric6DOFJointImpl3D::_classify() const { + if (!(_linear_locked(0) && _linear_locked(1) && _linear_locked(2))) { + return Mode::DRIVE; + } + const bool angular_active = _any_flag(PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_MOTOR) || + _any_flag(PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_SPRING); + if (!angular_active && _angular_locked(0) && _angular_locked(1) && _angular_locked(2)) { + return Mode::WELD; + } + return Mode::BALL; +} + +Quaternion Box3DGeneric6DOFJointImpl3D::_equilibrium_rotation() const { + const Vector3 euler( + axes[0].param[PhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_EQUILIBRIUM_POINT], + axes[1].param[PhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_EQUILIBRIUM_POINT], + axes[2].param[PhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_EQUILIBRIUM_POINT]); + return Basis::from_euler(euler).get_rotation_quaternion(); +} + +Vector3 Box3DGeneric6DOFJointImpl3D::_equilibrium_position() const { + return Vector3( + axes[0].param[PhysicsServer3D::G6DOF_JOINT_LINEAR_SPRING_EQUILIBRIUM_POINT], + axes[1].param[PhysicsServer3D::G6DOF_JOINT_LINEAR_SPRING_EQUILIBRIUM_POINT], + axes[2].param[PhysicsServer3D::G6DOF_JOINT_LINEAR_SPRING_EQUILIBRIUM_POINT]); +} + +Vector3 Box3DGeneric6DOFJointImpl3D::_motor_velocity(Param p_param, Flag p_gate) const { + Vector3 v; + for (int i = 0; i < 3; i++) { + if (axes[i].flag[p_gate]) { + v[i] = axes[i].param[p_param]; + } + } + return v; +} + +// Box3D motor velocities are world-space relative velocities; Godot's are per joint axis. +Vector3 Box3DGeneric6DOFJointImpl3D::_to_world_from_frame_a(const Vector3& p_local) const { + Quaternion frame = local_frame_a.basis.get_rotation_quaternion(); + if (get_body_a() != nullptr && get_body_a()->has_body_id()) { + frame = b3_to_godot(b3Body_GetRotation(get_body_a()->get_body_id())) * frame; + } + return frame.xform(p_local); +} + +b3JointId Box3DGeneric6DOFJointImpl3D::_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) { + mode = _classify(); + b3JointId id = b3_nullJointId; + + switch (mode) { + case Mode::WELD: { + b3WeldJointDef def = b3DefaultWeldJointDef(); + def.base.bodyIdA = p_body_a; + def.base.bodyIdB = p_body_b; + def.base.localFrameA = p_local_frame_a; + def.base.localFrameB = p_local_frame_b; + id = b3CreateWeldJoint(p_world_id, &def); + } break; + + case Mode::BALL: { + b3SphericalJointDef def = b3DefaultSphericalJointDef(); + def.base.bodyIdA = p_body_a; + def.base.bodyIdB = p_body_b; + def.base.localFrameA = remap_twist(p_local_frame_a); + def.base.localFrameB = remap_twist(p_local_frame_b); + + const AxisState& twist = axes[Vector3::AXIS_X]; + if (twist.flag[PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT]) { + def.enableTwistLimit = true; + def.lowerTwistAngle = (float)twist.param[PhysicsServer3D::G6DOF_JOINT_ANGULAR_LOWER_LIMIT]; + def.upperTwistAngle = (float)twist.param[PhysicsServer3D::G6DOF_JOINT_ANGULAR_UPPER_LIMIT]; + } + double cone = -1.0; + for (int i = Vector3::AXIS_Y; i <= Vector3::AXIS_Z; i++) { + if (axes[i].flag[PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT]) { + cone = MAX(cone, MAX(Math::abs(axes[i].param[PhysicsServer3D::G6DOF_JOINT_ANGULAR_LOWER_LIMIT]), + Math::abs(axes[i].param[PhysicsServer3D::G6DOF_JOINT_ANGULAR_UPPER_LIMIT]))); + } + } + if (cone >= 0.0) { + def.enableConeLimit = true; + def.coneAngle = (float)CLAMP(cone, 0.0, Math_PI); + } + if (axes[Vector3::AXIS_Y].flag[PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT] != + axes[Vector3::AXIS_Z].flag[PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT]) { + WARN_PRINT_ONCE("Box3D: Generic6DOFJoint3D ball mode uses one symmetric swing cone; angular Y/Z limits are merged."); + } + + def.enableSpring = _any_flag(PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_SPRING); + def.hertz = (float)_max_param(PhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_STIFFNESS, PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_SPRING); + def.dampingRatio = (float)_max_param(PhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_DAMPING, PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_SPRING); + def.targetRotation = godot_to_b3(TWIST_REMAP.inverse() * _equilibrium_rotation() * TWIST_REMAP); + + def.enableMotor = _any_flag(PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_MOTOR); + def.maxMotorTorque = (float)_max_param(PhysicsServer3D::G6DOF_JOINT_ANGULAR_MOTOR_FORCE_LIMIT, PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_MOTOR); + def.motorVelocity = godot_to_b3(_to_world_from_frame_a(_motor_velocity(PhysicsServer3D::G6DOF_JOINT_ANGULAR_MOTOR_TARGET_VELOCITY, PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_MOTOR))); + id = b3CreateSphericalJoint(p_world_id, &def); + } break; + + case Mode::DRIVE: { + b3MotorJointDef def = b3DefaultMotorJointDef(); + def.base.bodyIdA = p_body_a; + def.base.bodyIdB = p_body_b; + def.base.localFrameA = godot_to_b3_transform(local_frame_a * Transform3D(Basis(_equilibrium_rotation()), _equilibrium_position())); + def.base.localFrameB = p_local_frame_b; + id = b3CreateMotorJoint(p_world_id, &def); + if (_any_flag(PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT)) { + for (int i = 0; i < 3; i++) { + const AxisState& a = axes[i]; + if (a.flag[PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT] && !_linear_locked(i) && + a.param[PhysicsServer3D::G6DOF_JOINT_LINEAR_LOWER_LIMIT] < a.param[PhysicsServer3D::G6DOF_JOINT_LINEAR_UPPER_LIMIT]) { + WARN_PRINT_ONCE("Box3D: Generic6DOFJoint3D drive mode does not enforce linear limits."); + } + } + } + } break; + } + return id; +} + +// Values that can change every tick without rebuilding the joint (keeps the solver warm start). +void Box3DGeneric6DOFJointImpl3D::_apply_live() { + if (!has_joint_id()) { + return; + } + const b3JointId id = get_joint_id(); + switch (mode) { + case Mode::WELD: + break; + + case Mode::BALL: + b3SphericalJoint_EnableSpring(id, _any_flag(PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_SPRING)); + b3SphericalJoint_SetSpringHertz(id, (float)_max_param(PhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_STIFFNESS, PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_SPRING)); + b3SphericalJoint_SetSpringDampingRatio(id, (float)_max_param(PhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_DAMPING, PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_SPRING)); + b3SphericalJoint_SetTargetRotation(id, godot_to_b3(TWIST_REMAP.inverse() * _equilibrium_rotation() * TWIST_REMAP)); + b3SphericalJoint_EnableMotor(id, _any_flag(PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_MOTOR)); + b3SphericalJoint_SetMaxMotorTorque(id, (float)_max_param(PhysicsServer3D::G6DOF_JOINT_ANGULAR_MOTOR_FORCE_LIMIT, PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_MOTOR)); + b3SphericalJoint_SetMotorVelocity(id, godot_to_b3(_to_world_from_frame_a(_motor_velocity(PhysicsServer3D::G6DOF_JOINT_ANGULAR_MOTOR_TARGET_VELOCITY, PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_MOTOR)))); + break; + + case Mode::DRIVE: { + const double max_force = _max_param(PhysicsServer3D::G6DOF_JOINT_LINEAR_MOTOR_FORCE_LIMIT, PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_SPRING); + const double max_torque = _max_param(PhysicsServer3D::G6DOF_JOINT_ANGULAR_MOTOR_FORCE_LIMIT, PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_SPRING); + const bool linear_spring = _any_flag(PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_SPRING); + const bool angular_spring = _any_flag(PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_SPRING); + b3MotorJoint_SetLinearHertz(id, linear_spring ? (float)_max_param(PhysicsServer3D::G6DOF_JOINT_LINEAR_SPRING_STIFFNESS, PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_SPRING) : 0.0f); + b3MotorJoint_SetLinearDampingRatio(id, (float)_max_param(PhysicsServer3D::G6DOF_JOINT_LINEAR_SPRING_DAMPING, PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_SPRING)); + b3MotorJoint_SetMaxSpringForce(id, linear_spring ? (max_force > 0.0 ? (float)max_force : FLT_MAX) : 0.0f); + b3MotorJoint_SetAngularHertz(id, angular_spring ? (float)_max_param(PhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_STIFFNESS, PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_SPRING) : 0.0f); + b3MotorJoint_SetAngularDampingRatio(id, (float)_max_param(PhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_DAMPING, PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_SPRING)); + b3MotorJoint_SetMaxSpringTorque(id, angular_spring ? (max_torque > 0.0 ? (float)max_torque : FLT_MAX) : 0.0f); + + const bool linear_motor = _any_flag(PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_MOTOR); + const bool angular_motor = _any_flag(PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_MOTOR); + b3MotorJoint_SetLinearVelocity(id, godot_to_b3(_to_world_from_frame_a(_motor_velocity(PhysicsServer3D::G6DOF_JOINT_LINEAR_MOTOR_TARGET_VELOCITY, PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_MOTOR)))); + b3MotorJoint_SetMaxVelocityForce(id, linear_motor ? (float)_max_param(PhysicsServer3D::G6DOF_JOINT_LINEAR_MOTOR_FORCE_LIMIT, PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_MOTOR) : 0.0f); + b3MotorJoint_SetAngularVelocity(id, godot_to_b3(_to_world_from_frame_a(_motor_velocity(PhysicsServer3D::G6DOF_JOINT_ANGULAR_MOTOR_TARGET_VELOCITY, PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_MOTOR)))); + b3MotorJoint_SetMaxVelocityTorque(id, angular_motor ? (float)_max_param(PhysicsServer3D::G6DOF_JOINT_ANGULAR_MOTOR_FORCE_LIMIT, PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_MOTOR) : 0.0f); + + b3Joint_SetLocalFrameA(id, godot_to_b3_transform(local_frame_a * Transform3D(Basis(_equilibrium_rotation()), _equilibrium_position()))); + // Frame A moving under a sleeping body pair must wake them, or the new target is ignored. + b3Body_SetAwake(b3Joint_GetBodyB(id), true); + } break; + } +} + +double Box3DGeneric6DOFJointImpl3D::get_param(Vector3::Axis p_axis, Param p_param) const { + ERR_FAIL_INDEX_V((int)p_axis, 3, 0.0); + ERR_FAIL_INDEX_V((int)p_param, (int)PhysicsServer3D::G6DOF_JOINT_MAX, 0.0); + return axes[p_axis].param[p_param]; +} + +void Box3DGeneric6DOFJointImpl3D::set_param(Vector3::Axis p_axis, Param p_param, double p_value) { + ERR_FAIL_INDEX((int)p_axis, 3); + ERR_FAIL_INDEX((int)p_param, (int)PhysicsServer3D::G6DOF_JOINT_MAX); + if (axes[p_axis].param[p_param] == p_value) { + return; + } + axes[p_axis].param[p_param] = p_value; + + switch (p_param) { + case PhysicsServer3D::G6DOF_JOINT_LINEAR_LOWER_LIMIT: + case PhysicsServer3D::G6DOF_JOINT_LINEAR_UPPER_LIMIT: + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_LOWER_LIMIT: + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_UPPER_LIMIT: + rebuild(); + _apply_live(); + break; + case PhysicsServer3D::G6DOF_JOINT_LINEAR_LIMIT_SOFTNESS: + case PhysicsServer3D::G6DOF_JOINT_LINEAR_RESTITUTION: + case PhysicsServer3D::G6DOF_JOINT_LINEAR_DAMPING: + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_LIMIT_SOFTNESS: + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_DAMPING: + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_RESTITUTION: + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_FORCE_LIMIT: + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_ERP: + // Bullet-era limit tuning; Box3D limits are rigid with engine-wide softness. + break; + default: + _apply_live(); + break; + } +} + +bool Box3DGeneric6DOFJointImpl3D::get_flag(Vector3::Axis p_axis, Flag p_flag) const { + ERR_FAIL_INDEX_V((int)p_axis, 3, false); + ERR_FAIL_INDEX_V((int)p_flag, (int)PhysicsServer3D::G6DOF_JOINT_FLAG_MAX, false); + return axes[p_axis].flag[p_flag]; +} + +void Box3DGeneric6DOFJointImpl3D::set_flag(Vector3::Axis p_axis, Flag p_flag, bool p_enable) { + ERR_FAIL_INDEX((int)p_axis, 3); + ERR_FAIL_INDEX((int)p_flag, (int)PhysicsServer3D::G6DOF_JOINT_FLAG_MAX); + if (axes[p_axis].flag[p_flag] == p_enable) { + return; + } + axes[p_axis].flag[p_flag] = p_enable; + // Flags can change which Box3D joint represents this configuration (and the BALL limits), + // so rebuild; Godot sets them once after creation, not per tick. + rebuild(); + _apply_live(); +} diff --git a/src/joints/box3d_generic_6dof_joint_impl_3d.hpp b/src/joints/box3d_generic_6dof_joint_impl_3d.hpp new file mode 100644 index 0000000..af6d75d --- /dev/null +++ b/src/joints/box3d_generic_6dof_joint_impl_3d.hpp @@ -0,0 +1,72 @@ +#pragma once + +#include "box3d_joint_impl_3d.hpp" + +// GENERIC_6DOF_JOINT mapped onto the Box3D joint that matches its configuration. Box3D has no +// general 6-DOF constraint, but the configurations physics rigs actually use each have a native +// equivalent; the joint is rebuilt whenever a flag or limit changes which one applies: +// +// BALL — all three linear axes locked (limit on, lower == upper), angular not fully locked +// → b3SphericalJoint. The joint X axis is the twist axis (angular X limits = twist +// range); the swing cone is symmetric, its half-angle the largest |limit| of angular +// Y/Z. Angular spring → rotation spring toward the angular equilibrium points (Euler, +// Godot's default YXZ order). Angular motor → torque-capped velocity motor: target +// velocity per axis in joint frame A, force limit = max torque (N·m). This is the +// muscle primitive: drive the motor velocity every tick from a PD, cap = strength. +// WELD — every axis locked and no angular motor/spring → b3WeldJoint (rigid at creation pose). +// DRIVE — any linear axis not locked → b3MotorJoint: springs pull frame B onto frame A shifted +// by the equilibrium points, force/torque capped by the motor force limits (0 = no +// cap); motor target velocities add a velocity drive. Linear limits are not enforced. +// +// Box3D units: spring STIFFNESS is the spring frequency in Hz and DAMPING the damping ratio +// (not N/m and N·s/m) — Box3D's soft constraints are frequency-based and mass-independent. +class Box3DGeneric6DOFJointImpl3D final : public Box3DJointImpl3D { +public: + using Param = PhysicsServer3D::G6DOFJointAxisParam; + using Flag = PhysicsServer3D::G6DOFJointAxisFlag; + + Box3DGeneric6DOFJointImpl3D( + Box3DBodyImpl3D* p_body_a, + Box3DBodyImpl3D* p_body_b, + const Transform3D& p_local_frame_a, + const Transform3D& p_local_frame_b); + + PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_6DOF; } + + double get_param(Vector3::Axis p_axis, Param p_param) const; + + void set_param(Vector3::Axis p_axis, Param p_param, double p_value); + + bool get_flag(Vector3::Axis p_axis, Flag p_flag) const; + + void set_flag(Vector3::Axis p_axis, Flag p_flag, bool p_enable); + +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: + enum class Mode { + BALL, + WELD, + DRIVE, + }; + + struct AxisState { + double param[PhysicsServer3D::G6DOF_JOINT_MAX] = {}; + bool flag[PhysicsServer3D::G6DOF_JOINT_FLAG_MAX] = {}; + }; + + Mode _classify() const; + bool _linear_locked(int p_axis) const; + bool _angular_locked(int p_axis) const; + bool _any_flag(Flag p_flag) const; + double _max_param(Param p_param, Flag p_gate) const; + Quaternion _equilibrium_rotation() const; + Vector3 _equilibrium_position() const; + Vector3 _motor_velocity(Param p_param, Flag p_gate) const; + Vector3 _to_world_from_frame_a(const Vector3& p_local) const; + void _apply_live(); + + AxisState axes[3]; + Mode mode = Mode::WELD; +}; diff --git a/src/servers/box3d_physics_server_3d.cpp b/src/servers/box3d_physics_server_3d.cpp index 01565a2..903f2bb 100644 --- a/src/servers/box3d_physics_server_3d.cpp +++ b/src/servers/box3d_physics_server_3d.cpp @@ -6,6 +6,7 @@ #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 "../joints/box3d_generic_6dof_joint_impl_3d.hpp" #include "../misc/type_conversions.hpp" #include "../objects/box3d_area_impl_3d.hpp" #include "../objects/box3d_body_impl_3d.hpp" @@ -1124,21 +1125,41 @@ double Box3DPhysicsServer3D::_cone_twist_joint_get_param(const RID& p_joint, Phy } 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) { - ERR_FAIL_MSG("Box3D: Generic6DOFJoint3D is not supported; use PinJoint3D, HingeJoint3D, or SliderJoint3D instead."); + _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(Box3DGeneric6DOFJointImpl3D(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::_generic_6dof_joint_set_param(const RID& p_joint, Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisParam p_param, double p_value) { + auto* joint = dynamic_cast(joint_owner.get_or_null(p_joint)); + ERR_FAIL_NULL(joint); + joint->set_param(p_axis, p_param, p_value); } double Box3DPhysicsServer3D::_generic_6dof_joint_get_param(const RID& p_joint, Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisParam 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_axis, p_param); } void Box3DPhysicsServer3D::_generic_6dof_joint_set_flag(const RID& p_joint, Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisFlag p_flag, bool p_enable) { + auto* joint = dynamic_cast(joint_owner.get_or_null(p_joint)); + ERR_FAIL_NULL(joint); + joint->set_flag(p_axis, p_flag, p_enable); } bool Box3DPhysicsServer3D::_generic_6dof_joint_get_flag(const RID& p_joint, Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisFlag p_flag) const { - return false; + auto* joint = dynamic_cast(joint_owner.get_or_null(p_joint)); + ERR_FAIL_NULL_V(joint, false); + return joint->get_flag(p_axis, p_flag); } PhysicsServer3D::JointType Box3DPhysicsServer3D::_joint_get_type(const RID& p_joint) const { -- 2.55.0