Foxyz/VR-Mess
1
0 Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
ca9a299fe880f16fe83cf03d2122c3b83bef7708
VR-Mess/.vendor/box3d-generic-6dof.patch
T
FoxyzandClaude Opus 5.5 3a990c6dbe Body rebuild step 0: Generic6DOFJoint3D on Box3D (fork patch 2) — ball w/ cone/twist + torque-capped muscle motor, weld, drive 
- addons/godot-box3d/bin/libgodot-box3d.so rebuilt from fork branch generic-6dof-joint
  (weld patch + 6DOF); .vendor/box3d-generic-6dof.patch exported
- SixDofSpike: type, ball pivot/swing, cone+twist limits, muscle holds 2.94 N·m lever at 10 N·m
  cap and fails at 1.5, tracks moving target, drive follows + push-off reaction, weld rigid,
  live flag toggles — PASS x3; all existing spikes PASS x3 on the new binary
- research/godot-box3d-6dof.md (mapping, units, C# muscle recipe); HANDOFF/AGENTS pointers

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
2026-10-02 05:10:45 +03:00

456 lines
22 KiB
Diff
Raw Blame History

From b0777e85b418ee093364906b1f025ef4eaec8881 Mon Sep 17 00:00:00 2001
From: Foxyz <timka1302@gmail.com>
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 <noreply@anthropic.com>
---
.../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 <box3d/box3d.h>
+
+#include <cfloat>
+
+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<Box3DGeneric6DOFJointImpl3D*>(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<Box3DGeneric6DOFJointImpl3D*>(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<Box3DGeneric6DOFJointImpl3D*>(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<Box3DGeneric6DOFJointImpl3D*>(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
Reference in new issue View Git Blame Copy Permalink