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>
This commit is contained in:
FoxyzandClaude Opus 5.5 committed
1 parent
329b7bf143
commit
3a990c6dbe
7 files changed
+896
-2
No files matched your search
@@ -0,0 +1,455 @@
|
|||||||
|
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
|
||||||
|
|
||||||
@@ -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
|
`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
|
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).
|
(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
|
Jolt (built-in) is the fallback: reverting is
|
||||||
the one project-setting line, and all gameplay code must stay standard-Godot-physics-node
|
the one project-setting line, and all gameplay code must stay standard-Godot-physics-node
|
||||||
based so that flip is lossless.
|
based so that flip is lossless.
|
||||||
|
|||||||
+6
-2
@@ -26,6 +26,9 @@ TERMINOLOGY (owner-corrected — use precisely):
|
|||||||
source + rebuild recipe + patch in `.vendor/` (`.vendor/box3d-weld.patch`,
|
source + rebuild recipe + patch in `.vendor/` (`.vendor/box3d-weld.patch`,
|
||||||
`.vendor/godot-box3d/`, see `research/godot-box3d-weld.md`) — **only the Linux binary is
|
`.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).
|
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
|
Jolt fallback = flip that one project-setting line; all gameplay code stays standard
|
||||||
RigidBody3D/StaticBody3D/CharacterBody3D so the flip is lossless.
|
RigidBody3D/StaticBody3D/CharacterBody3D so the flip is lossless.
|
||||||
- Runtime stack: **WiVRN + WayVR** on Linux. Headset + runtime support hand tracking (treat as available).
|
- 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
|
(`--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:
|
collapse spike takes seconds instead of minutes). Spikes and their pass lines:
|
||||||
`physics_spike` "SPIKE: overall PASS", `weld_spike` "WELD: …", `grab_spike` "GRAB: …",
|
`physics_spike` "SPIKE: overall PASS", `weld_spike` "WELD: …", `grab_spike` "GRAB: …",
|
||||||
`body_spike` "BODY: …", `collapse_spike` "COLLAPSE: …", `arm_spike` "ARM: overall PASS".
|
`body_spike` "BODY: …", `collapse_spike` "COLLAPSE: …", `arm_spike` "ARM: overall PASS",
|
||||||
All six PASS 3× as of this handoff. Headless asset import:
|
`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`.
|
`godot-mono --headless --import`.
|
||||||
- Engine logs: `~/.local/share/godot/app_userdata/VR Messs/logs/godot.log` — **read after user
|
- Engine logs: `~/.local/share/godot/app_userdata/VR Messs/logs/godot.log` — **read after user
|
||||||
playtests instead of guessing**.
|
playtests instead of guessing**.
|
||||||
|
|||||||
Binary file not shown.
@@ -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.
|
||||||
@@ -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")
|
||||||
@@ -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<float> Tick, float Duration, Func<bool> 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<Generic6DofJoint3D> 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<Generic6DofJoint3D.Flag, bool>, Action<Generic6DofJoint3D.Param, float>> 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<Generic6DofJoint3D.Param, float>[] { 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;
|
||||||
|
}
|
||||||
|
}
|
||||||
Reference in new issue
Block a user