diff --git a/.vendor/box3d-generic-6dof.patch b/.vendor/box3d-generic-6dof.patch new file mode 100644 index 0000000..2ae5812 --- /dev/null +++ b/.vendor/box3d-generic-6dof.patch @@ -0,0 +1,455 @@ +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 + diff --git a/AGENTS.md b/AGENTS.md index 9f9a9ac..eaa97f9 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -25,6 +25,9 @@ embodiment/view system in particular is theory until proven. `https://github.com/Foxyz1302/godot-box3d` (branch `rigid-weld-joint`, carries the weld patch; PR to upstream pending, owner-side) — and it may gain more additions in the future (rebuild recipe + source in `.vendor/`, only the Linux binary is patched locally). + Patches so far: ConeTwistJoint3D → rigid weld (`research/godot-box3d-weld.md`), Generic6DOFJoint3D → + ball/weld/drive with torque-capped muscle motors (`research/godot-box3d-6dof.md`, regression + `scenes/sixdof_spike.tscn`). Jolt (built-in) is the fallback: reverting is the one project-setting line, and all gameplay code must stay standard-Godot-physics-node based so that flip is lossless. diff --git a/HANDOFF.md b/HANDOFF.md index 1184769..914380a 100644 --- a/HANDOFF.md +++ b/HANDOFF.md @@ -26,6 +26,9 @@ TERMINOLOGY (owner-corrected — use precisely): source + rebuild recipe + patch in `.vendor/` (`.vendor/box3d-weld.patch`, `.vendor/godot-box3d/`, see `research/godot-box3d-weld.md`) — **only the Linux binary is patched**; Windows/macOS binaries are stock (ConeTwist still errors there until rebuilt). + **Second patch (2026-10-02): `Generic6DOFJoint3D` → Box3D ball (spherical, cone/twist limits, torque-capped + motor = MUSCLE) / weld / drive (motor joint)** — fork branch `generic-6dof-joint`, `.vendor/box3d-generic-6dof.patch`, + details + C# muscle recipe in `research/godot-box3d-6dof.md`. Spring stiffness/damping are Box3D Hz/ratio. Jolt fallback = flip that one project-setting line; all gameplay code stays standard RigidBody3D/StaticBody3D/CharacterBody3D so the flip is lossless. - Runtime stack: **WiVRN + WayVR** on Linux. Headset + runtime support hand tracking (treat as available). @@ -34,8 +37,9 @@ TERMINOLOGY (owner-corrected — use precisely): (`--fixed-fps 90` runs physics faster than real time at the real 90 Hz tick — the 49-scenario collapse spike takes seconds instead of minutes). Spikes and their pass lines: `physics_spike` "SPIKE: overall PASS", `weld_spike` "WELD: …", `grab_spike` "GRAB: …", - `body_spike` "BODY: …", `collapse_spike` "COLLAPSE: …", `arm_spike` "ARM: overall PASS". - All six PASS 3× as of this handoff. Headless asset import: + `body_spike` "BODY: …", `collapse_spike` "COLLAPSE: …", `arm_spike` "ARM: overall PASS", + `sixdof_spike` "6DOF: overall PASS" (run FIRST with `weld_spike` whenever the addon binary changes). + All seven PASS 3× as of this handoff. Headless asset import: `godot-mono --headless --import`. - Engine logs: `~/.local/share/godot/app_userdata/VR Messs/logs/godot.log` — **read after user playtests instead of guessing**. diff --git a/addons/godot-box3d/bin/libgodot-box3d.so b/addons/godot-box3d/bin/libgodot-box3d.so index dacef9c..e6b6b04 100644 Binary files a/addons/godot-box3d/bin/libgodot-box3d.so and b/addons/godot-box3d/bin/libgodot-box3d.so differ diff --git a/research/godot-box3d-6dof.md b/research/godot-box3d-6dof.md new file mode 100644 index 0000000..37d8b47 --- /dev/null +++ b/research/godot-box3d-6dof.md @@ -0,0 +1,48 @@ +# Addon patch 2: Generic6DOFJoint3D on Box3D (2026-10-02) + +Second local patch to the godot-box3d fork (after the ConeTwist→weld patch, `godot-box3d-weld.md`). Purpose: +the joint primitives for the Project-TX-style muscle ragdoll (`nimso-project-tx.md`, `vr-physics-rig-hexabody.md` §4). + +- Fork branch `generic-6dof-joint` (on top of `rigid-weld-joint`) in `.vendor/godot-box3d`, commit "Expose Box3D + joints through Godot's Generic6DOFJoint3D"; patch file `.vendor/box3d-generic-6dof.patch`. NOT pushed to + `github.com/Foxyz1302/godot-box3d` yet (owner decision). Linux binary only (same rebuild recipe as the weld + patch: `cmake --build build --parallel 12` in `.vendor/godot-box3d`, copy `bin/libgodot-box3d.so` to + `addons/godot-box3d/bin/`; stock binaries stay in `addons/godot-box3d/bin-stock/`). +- Files: `src/joints/box3d_generic_6dof_joint_impl_3d.{hpp,cpp}` (new), `src/servers/box3d_physics_server_3d.cpp` + (`_joint_make_generic_6dof` + param/flag get/set dispatch; was a hard error). + +## Mapping (configuration-based — Box3D has no general 6-DOF constraint) + +| Godot configuration | Box3D joint | Notes | +|---|---|---| +| all 3 linear axes locked (limit on, lower ≥ upper, no linear spring/motor), angular not fully locked or angular motor/spring on | `b3SphericalJoint` = **BALL** | joint **X = twist axis** (angular X limits = twist range); swing cone symmetric, half-angle = largest \|limit\| of angular Y/Z (warned if Y/Z differ); angular spring → rotation spring to the angular equilibrium (Euler, Godot default YXZ); angular **motor = torque-capped velocity motor** | +| everything locked, no angular motor/spring (the Godot default) | `b3WeldJoint` = **WELD** | rigid at the creation pose | +| any linear axis free/springy/motored | `b3MotorJoint` = **DRIVE** | springs pull frame B onto frame A shifted by the equilibrium points (linear + Euler); force/torque caps = Linear/Angular **MotorForceLimit** (0 = uncapped); motor target velocities add a velocity drive; linear limits NOT enforced (warned) | + +- **Units differ from Godot/Jolt**: spring STIFFNESS = frequency in **Hz**, DAMPING = **damping ratio** (Box3D + soft constraints are mass-independent). Choose ~5–15 Hz, ratio ~1. +- Motor target velocities are per joint axis in **joint frame A**; the impl converts to Box3D's world-space relative + angular/linear velocity at set time using body A's current rotation → set them EVERY TICK (they are a PD output). +- Flags and limits **rebuild** the Box3D joint (warm start lost — set them once); motor velocities, force limits, + springs and equilibrium points apply **live**. Configure before `AddChild` (as with every joint here). +- Bullet-era params (limit softness/restitution/damping, ERP, angular force limit) are ignored. + +## Muscle recipe (C#, used by `scripts/SixDofSpike.cs`) + +```csharp +// BALL joint: angular limits as the anatomical range, EnableMotor on X/Y/Z, AngularMotorForceLimit = strength (N·m) +Quaternion rel = frameAWorld.Inverse() * frameBWorld; // current joint rotation, frame-A coordinates +Vector3 err = BodyMath.RotationError(targetRel, rel); // axis*angle in frame-A coordinates +Vector3 w = (err * Kp).LimitLength(maxSpin); // PD velocity +joint.SetParamX(Generic6DofJoint3D.Param.AngularMotorTargetVelocity, w.X); // …Y, …Z +``` +The solver applies at most `AngularMotorForceLimit` torque per step — strength is a real cap, leverage emerges. + +## Proof — `scenes/sixdof_spike.tscn` ("6DOF: overall PASS", 3× identical) + +server type `Type6Dof`; BALL pivot holds (≤ 0.7 mm) while swinging 100°; cone 30° / twist 10° hold under hard torque +(31.3° / 11.6° peak — Box3D soft-limit overshoot); muscle PD holds a 1 kg × 0.6 m lever (2.94 N·m gravity load) +horizontal with a 10 N·m cap (0.0°) and FAILS with 1.5 N·m (drops 107°); tracks a 60° raise to 0.0°; DRIVE follows a +moving target to 0.000 m; a hand driven into a wall never penetrates it and pushes the 10 kg anchor body AWAY +(−0.82 m — push-off reaction; pulling toward a fixed point is the climbing case); all-locked = rigid (0.13°); 90 +live flag toggles stay finite. All existing spikes (weld, physics, grab, body, collapse, arm) PASS 3× on the new binary. diff --git a/scenes/sixdof_spike.tscn b/scenes/sixdof_spike.tscn new file mode 100644 index 0000000..cc68bcc --- /dev/null +++ b/scenes/sixdof_spike.tscn @@ -0,0 +1,6 @@ +[gd_scene load_steps=2 format=3] + +[ext_resource type="Script" path="res://scripts/SixDofSpike.cs" id="1_spike"] + +[node name="SixDofSpike" type="Node3D"] +script = ExtResource("1_spike") diff --git a/scripts/SixDofSpike.cs b/scripts/SixDofSpike.cs new file mode 100644 index 0000000..33624c8 --- /dev/null +++ b/scripts/SixDofSpike.cs @@ -0,0 +1,378 @@ +using System; +using System.Collections.Generic; +using Godot; + +// Headless regression for the forked addon's Generic6DOFJoint3D → Box3D mapping +// (ball = b3SphericalJoint with cone/twist limits + torque-capped motor; drive = b3MotorJoint; +// all-locked = weld). Run first whenever the addon binary changes. +// godot-mono --headless --fixed-fps 90 --path . scenes/sixdof_spike.tscn +public partial class SixDofSpike : Node3D +{ + private const float SwingLimitDeg = 30f; + private const float TwistLimitDeg = 10f; + private const float LimitToleranceDeg = 3f; + private const float PivotLimit = 0.01f; + private const float LeverMass = 1f; + private const float LeverLength = 0.6f; + private const float StrongMuscle = 10f; + private const float WeakMuscle = 1.5f; + private const float MuscleKp = 20f; + private const float MuscleMaxSpin = 8f; + + private readonly List<(string Name, Action Tick, float Duration, Func Finish)> _phases = new(); + private int _phase = -1; + private float _t; + private bool _allOk = true; + private Node3D _stage; + + public override void _Ready() + { + _phases.Add(("joint type", _ => { }, 0.1f, PhaseType)); + _phases.Add(("ball holds pivot and swings freely", BallTick, 1.5f, BallFinish)); + _phases.Add(("cone + twist limits hold under torque", LimitTick, 1.5f, LimitFinish)); + _phases.Add(("strong muscle holds a 2.9 N·m lever horizontal", t => MuscleTick(t, false), 1.5f, () => MuscleFinish(true))); + _phases.Add(("weak muscle cannot hold it", t => MuscleTick(t, false), 1.5f, () => MuscleFinish(false))); + _phases.Add(("muscle tracks a moving target", t => MuscleTick(t, true), 1.8f, MuscleTrackFinish)); + _phases.Add(("drive follows a moving target", DriveTick, 1.5f, DriveFollowFinish)); + _phases.Add(("hand pushing into a wall pushes the anchor body away (reaction)", DriveBlockedTick, 1.5f, DriveBlockedFinish)); + _phases.Add(("all-locked config is rigid", WeldTick, 1.5f, WeldFinish)); + _phases.Add(("live flag toggling stays finite", ToggleTick, 1f, ToggleFinish)); + Next(); + } + + public override void _PhysicsProcess(double delta) + { + if (_phase >= _phases.Count) + return; + float dt = (float)delta; + _t += dt; + _phases[_phase].Tick(dt); + if (_t >= _phases[_phase].Duration) + { + bool ok = _phases[_phase].Finish(); + _allOk &= ok; + GD.Print($"6DOF: {_phases[_phase].Name} → {(ok ? "PASS" : "FAIL")}"); + Next(); + } + } + + private void Next() + { + _stage?.QueueFree(); + _stage = new Node3D(); + AddChild(_stage); + _t = 0f; + _phase++; + if (_phase >= _phases.Count) + { + GD.Print($"6DOF: overall {(_allOk ? "PASS — Generic6DOFJoint3D functional on patched Box3D" : "CHECK — inspect failures above")}"); + GetTree().Quit(); + return; + } + Setup(_phase); + } + + // ---- scene pieces ---- + private StaticBody3D _anchor; + private RigidBody3D _bodyA; + private RigidBody3D _box; + private Generic6DofJoint3D _joint; + private Basis _frameLocalA; + private Basis _frameLocalB; + private Vector3 _pivotLocalB; + private Vector3 _pivotWorld; + private float _maxPivotSep; + private float _maxSwing; + private float _maxTwist; + private float _swingRange; + private float _startAngle; + private float _maxErrDeg; + private float _endErrDeg; + private Vector3 _bodyAStart; + private bool _finite = true; + private StaticBody3D _wall; + + private RigidBody3D MakeBox(Vector3 size, float mass, Vector3 pos, float gravity = 1f) + { + var body = new RigidBody3D { Mass = mass, GravityScale = gravity, Position = pos }; + body.AddChild(new CollisionShape3D { Shape = new BoxShape3D { Size = size } }); + _stage.AddChild(body); + return body; + } + + private StaticBody3D MakeStatic(Vector3 size, Vector3 pos) + { + var body = new StaticBody3D { Position = pos }; + body.AddChild(new CollisionShape3D { Shape = new BoxShape3D { Size = size } }); + _stage.AddChild(body); + return body; + } + + // Configure BEFORE AddChild (the node pushes everything to the server on enter-tree). + private Generic6DofJoint3D MakeJoint(PhysicsBody3D a, PhysicsBody3D b, Vector3 at, Action configure) + { + var joint = new Generic6DofJoint3D { Position = at, NodeA = a.GetPath(), NodeB = b.GetPath(), ExcludeNodesFromCollision = true }; + configure(joint); + _stage.AddChild(joint); + _frameLocalA = a.GlobalTransform.Basis.Inverse() * joint.GlobalTransform.Basis; + _frameLocalB = b.GlobalTransform.Basis.Inverse() * joint.GlobalTransform.Basis; + _pivotLocalB = b.GlobalTransform.AffineInverse() * at; + _pivotWorld = at; + return joint; + } + + private static void ForAxes(Action, Action> body, Generic6DofJoint3D j) + { + body((f, v) => { j.SetFlagX(f, v); j.SetFlagY(f, v); j.SetFlagZ(f, v); }, + (p, v) => { j.SetParamX(p, v); j.SetParamY(p, v); j.SetParamZ(p, v); }); + } + + private void Setup(int phase) + { + _maxPivotSep = _maxSwing = _maxTwist = _swingRange = _maxErrDeg = _endErrDeg = 0f; + _finite = true; + switch (phase) + { + case 0: + _anchor = MakeStatic(Vector3.One * 0.1f, new Vector3(0, 3, 0)); + _box = MakeBox(new Vector3(0.1f, 0.4f, 0.1f), 1f, new Vector3(0, 2.8f, 0)); + _joint = MakeJoint(_anchor, _box, new Vector3(0, 3, 0), j => ForAxes((f, p) => f(Generic6DofJoint3D.Flag.EnableAngularLimit, false), j)); + break; + case 1: + _anchor = MakeStatic(Vector3.One * 0.1f, new Vector3(0, 3, 0)); + _box = MakeBox(new Vector3(0.1f, 0.4f, 0.1f), 1f, new Vector3(0, 2.8f, 0)); + _joint = MakeJoint(_anchor, _box, new Vector3(0, 3, 0), j => ForAxes((f, p) => f(Generic6DofJoint3D.Flag.EnableAngularLimit, false), j)); + _box.ApplyCentralImpulse(new Vector3(2.5f, 0, 0)); + _startAngle = SwingAngle(); + break; + case 2: + _anchor = MakeStatic(Vector3.One * 0.1f, new Vector3(0, 3, 0)); + // Twist axis = joint X; the box hangs along -Y, so the joint basis is rotated to point X down. + _box = MakeBox(new Vector3(0.1f, 0.4f, 0.1f), 1f, new Vector3(0, 2.8f, 0)); + _joint = MakeJoint(_anchor, _box, new Vector3(0, 3, 0), j => + { + j.Basis = new Basis(Vector3.Forward, Mathf.Pi / 2f); + j.SetParamX(Generic6DofJoint3D.Param.AngularLowerLimit, Mathf.DegToRad(-TwistLimitDeg)); + j.SetParamX(Generic6DofJoint3D.Param.AngularUpperLimit, Mathf.DegToRad(TwistLimitDeg)); + foreach (var set in new Action[] { j.SetParamY, j.SetParamZ }) + { + set(Generic6DofJoint3D.Param.AngularLowerLimit, Mathf.DegToRad(-SwingLimitDeg)); + set(Generic6DofJoint3D.Param.AngularUpperLimit, Mathf.DegToRad(SwingLimitDeg)); + } + }); + break; + case 3: + case 4: + case 5: + { + // Lever sticking out horizontally along +X from a static shoulder. + _anchor = MakeStatic(Vector3.One * 0.1f, new Vector3(0, 3, 0)); + _box = MakeBox(new Vector3(LeverLength, 0.06f, 0.06f), LeverMass, new Vector3(LeverLength * 0.5f, 3, 0)); + float cap = phase == 4 ? WeakMuscle : StrongMuscle; + _joint = MakeJoint(_anchor, _box, new Vector3(0, 3, 0), j => ForAxes((f, p) => + { + f(Generic6DofJoint3D.Flag.EnableAngularLimit, false); + f(Generic6DofJoint3D.Flag.EnableMotor, true); + p(Generic6DofJoint3D.Param.AngularMotorForceLimit, cap); + }, j)); + break; + } + case 6: + case 7: + { + _bodyA = MakeBox(new Vector3(0.3f, 0.5f, 0.2f), 10f, new Vector3(0, 1.5f, 0), gravity: 0f); + _box = MakeBox(new Vector3(0.04f, 0.09f, 0.12f), 0.4f, new Vector3(0.4f, 1.5f, 0), gravity: 0f); + _joint = MakeJoint(_bodyA, _box, new Vector3(0.4f, 1.5f, 0), j => ForAxes((f, p) => + { + f(Generic6DofJoint3D.Flag.EnableLinearLimit, false); + f(Generic6DofJoint3D.Flag.EnableAngularLimit, false); + f(Generic6DofJoint3D.Flag.EnableLinearSpring, true); + f(Generic6DofJoint3D.Flag.EnableAngularSpring, true); + p(Generic6DofJoint3D.Param.LinearSpringStiffness, 10f); + p(Generic6DofJoint3D.Param.LinearSpringDamping, 1f); + p(Generic6DofJoint3D.Param.LinearMotorForceLimit, 90f); + p(Generic6DofJoint3D.Param.AngularSpringStiffness, 10f); + p(Generic6DofJoint3D.Param.AngularSpringDamping, 1f); + p(Generic6DofJoint3D.Param.AngularMotorForceLimit, 5f); + }, j)); + _bodyAStart = _bodyA.GlobalPosition; + if (phase == 7) + _wall = MakeStatic(new Vector3(0.1f, 2f, 2f), new Vector3(0.6f, 1.5f, 0)); + break; + } + case 8: + _anchor = MakeStatic(Vector3.One * 0.1f, new Vector3(0, 3, 0)); + _box = MakeBox(new Vector3(0.1f, 0.4f, 0.1f), 1f, new Vector3(0, 2.8f, 0)); + _joint = MakeJoint(_anchor, _box, new Vector3(0, 3, 0), _ => { }); + break; + case 9: + _anchor = MakeStatic(Vector3.One * 0.1f, new Vector3(0, 3, 0)); + _box = MakeBox(new Vector3(0.1f, 0.4f, 0.1f), 1f, new Vector3(0, 2.8f, 0)); + _joint = MakeJoint(_anchor, _box, new Vector3(0, 3, 0), j => ForAxes((f, p) => f(Generic6DofJoint3D.Flag.EnableAngularLimit, false), j)); + break; + } + } + + // ---- measurements ---- + private Quaternion FrameA() => (_anchor != null ? _anchor.GlobalTransform.Basis : _bodyA.GlobalTransform.Basis).GetRotationQuaternion() * _frameLocalA.GetRotationQuaternion(); + private Quaternion FrameB() => _box.GlobalTransform.Basis.GetRotationQuaternion() * _frameLocalB.GetRotationQuaternion(); + private Quaternion Rel() => FrameA().Inverse() * FrameB(); + + private float SwingAngle() + { + Vector3 xa = FrameA() * Vector3.Right; + Vector3 xb = FrameB() * Vector3.Right; + return Mathf.RadToDeg(xa.AngleTo(xb)); + } + + private float TwistAngle() + { + Quaternion r = Rel(); + return Mathf.RadToDeg(Mathf.Abs(2f * Mathf.Atan2(r.X, r.W))); + } + + private float PivotSep() => (_box.GlobalTransform * _pivotLocalB).DistanceTo(_pivotWorld); + + private void TrackFinite() => _finite &= BodyMath.IsFinite(_box) && (_bodyA == null || !IsInstanceValid(_bodyA) || BodyMath.IsFinite(_bodyA)); + + // ---- phases ---- + private bool PhaseType() + { + PhysicsServer3D.JointType type = PhysicsServer3D.JointGetType(_joint.GetRid()); + GD.Print($"6DOF: server joint type {type}"); + return type == PhysicsServer3D.JointType.Type6Dof; + } + + private void BallTick(float dt) + { + TrackFinite(); + _maxPivotSep = Mathf.Max(_maxPivotSep, PivotSep()); + _swingRange = Mathf.Max(_swingRange, Mathf.Abs(SwingAngle() - _startAngle)); + } + + private bool BallFinish() + { + GD.Print($"6DOF: pivot separation max {_maxPivotSep:F4} m (limit {PivotLimit}), swing range {_swingRange:F1} deg (> 20)"); + return _finite && _maxPivotSep < PivotLimit && _swingRange > 20f; + } + + private void LimitTick(float dt) + { + TrackFinite(); + // Strong torques about all axes: swing pushes against the cone, spin against the twist range. + _box.ApplyTorque(new Vector3(6f, 3f, 6f) * Mathf.Sign(Mathf.Sin(_t * 4f))); + _maxSwing = Mathf.Max(_maxSwing, SwingAngle()); + _maxTwist = Mathf.Max(_maxTwist, TwistAngle()); + _maxPivotSep = Mathf.Max(_maxPivotSep, PivotSep()); + } + + private bool LimitFinish() + { + GD.Print($"6DOF: swing max {_maxSwing:F1} deg (cone {SwingLimitDeg}), twist max {_maxTwist:F1} deg (limit {TwistLimitDeg}), pivot {_maxPivotSep:F4} m"); + return _finite && _maxSwing < SwingLimitDeg + LimitToleranceDeg && _maxTwist < TwistLimitDeg + LimitToleranceDeg + && _maxSwing > SwingLimitDeg * 0.6f && _maxPivotSep < PivotLimit; + } + + private Quaternion _muscleTarget = Quaternion.Identity; + + // Muscle = PD on the relative rotation, written as the motor target velocity (joint-frame-A + // axes); the motor's torque cap is the strength. BodyMath.RotationError gives the error in + // frame-A coordinates because both quaternions are expressed relative to frame A. + private void MuscleTick(float dt, bool moving) + { + TrackFinite(); + _muscleTarget = moving + ? new Quaternion(Vector3.Back, Mathf.DegToRad(60f) * Mathf.Min(_t / 1f, 1f)) + : Quaternion.Identity; + Vector3 err = BodyMath.RotationError(_muscleTarget, Rel()); + Vector3 w = (err * MuscleKp).LimitLength(MuscleMaxSpin); + _joint.SetParamX(Generic6DofJoint3D.Param.AngularMotorTargetVelocity, w.X); + _joint.SetParamY(Generic6DofJoint3D.Param.AngularMotorTargetVelocity, w.Y); + _joint.SetParamZ(Generic6DofJoint3D.Param.AngularMotorTargetVelocity, w.Z); + float errDeg = Mathf.RadToDeg(err.Length()); + if (_t > 0.5f) + _maxErrDeg = Mathf.Max(_maxErrDeg, errDeg); + _endErrDeg = errDeg; + } + + private bool MuscleFinish(bool expectHold) + { + GD.Print($"6DOF: lever error after settle max {_maxErrDeg:F1} deg, end {_endErrDeg:F1} deg (gravity torque {LeverMass * 9.81f * LeverLength * 0.5f:F2} N·m)"); + return _finite && (expectHold ? _maxErrDeg < 3f : _maxErrDeg > 20f); + } + + private bool MuscleTrackFinish() + { + GD.Print($"6DOF: tracking a 60 deg raise: end error {_endErrDeg:F1} deg (limit 3)"); + return _finite && _endErrDeg < 3f; + } + + private Vector3 DriveTarget => new Vector3(0.4f + 0.5f * Mathf.Min(_t / 1f, 1f), 0, 0); + + private void DriveTick(float dt) + { + TrackFinite(); + // Equilibrium points are offsets of the target from the creation pose, in joint frame A. + Vector3 offset = DriveTarget - new Vector3(0.4f, 0, 0); + _joint.SetParamX(Generic6DofJoint3D.Param.LinearSpringEquilibriumPoint, offset.X); + _joint.SetParamY(Generic6DofJoint3D.Param.LinearSpringEquilibriumPoint, offset.Y); + _joint.SetParamZ(Generic6DofJoint3D.Param.LinearSpringEquilibriumPoint, offset.Z); + } + + private bool DriveFollowFinish() + { + Vector3 handRel = _bodyA.GlobalTransform.AffineInverse() * _box.GlobalPosition; + float err = handRel.DistanceTo(DriveTarget); + GD.Print($"6DOF: hand at {handRel} vs target {DriveTarget}: error {err:F3} m (limit 0.03)"); + return _finite && err < 0.03f; + } + + private float _maxHandX; + + private void DriveBlockedTick(float dt) + { + DriveTick(dt); + _maxHandX = Mathf.Max(_maxHandX, _box.GlobalPosition.X); + } + + private bool DriveBlockedFinish() + { + float moved = _bodyA.GlobalPosition.X - _bodyAStart.X; + // Hand half-depth 0.02 → its centre can't pass x = 0.53 without penetrating the wall face. + GD.Print($"6DOF: hand x max {_maxHandX:F3} (wall face 0.55, hand half-width 0.02), anchor body moved {moved:F3} m (< -0.02 = pushed away)"); + return _finite && _maxHandX < 0.545f && moved < -0.02f; + } + + private void WeldTick(float dt) + { + TrackFinite(); + _box.ApplyTorque(new Vector3(0.5f, 0.5f, 0.5f)); + _box.ApplyCentralForce(new Vector3(5f, 0, 0)); + _maxSwing = Mathf.Max(_maxSwing, Mathf.RadToDeg(Rel().GetAngle())); + _maxPivotSep = Mathf.Max(_maxPivotSep, PivotSep()); + } + + private bool WeldFinish() + { + GD.Print($"6DOF: rotation drift max {_maxSwing:F2} deg (limit 2), pivot {_maxPivotSep:F4} m"); + return _finite && _maxSwing < 2f && _maxPivotSep < PivotLimit; + } + + private int _toggles; + + private void ToggleTick(float dt) + { + TrackFinite(); + _toggles++; + bool on = _toggles % 2 == 0; + _joint.SetFlagY(Generic6DofJoint3D.Flag.EnableMotor, on); + _joint.SetFlagX(Generic6DofJoint3D.Flag.EnableAngularLimit, !on); + _box.ApplyTorque(new Vector3(1f, 0, 1f)); + } + + private bool ToggleFinish() + { + GD.Print($"6DOF: {_toggles} live flag toggles, finite {_finite}, pivot {PivotSep():F4} m"); + return _finite && PivotSep() < 0.05f; + } +}