diff --git a/doc/classes/Generic6DOFJoint3D.xml b/doc/classes/Generic6DOFJoint3D.xml index 0b6a805b17..8f30c0e772 100644 --- a/doc/classes/Generic6DOFJoint3D.xml +++ b/doc/classes/Generic6DOFJoint3D.xml @@ -10,6 +10,18 @@ + + + + Clears the quaternion angular target. After clearing, the joint is driven by [constant PARAM_ANGULAR_SPRING_EQUILIBRIUM_POINT] on each axis as configured. + + + + + + Returns the joint's current angular target as a body-space quaternion. If no explicit target was set via [method set_angular_target_rotation], the value is derived from [constant PARAM_ANGULAR_SPRING_EQUILIBRIUM_POINT] on backends that support it; other backends return the identity quaternion. + + @@ -46,6 +58,20 @@ + + + + Returns [code]true[/code] if a quaternion angular target was explicitly set via [method set_angular_target_rotation] and has not been cleared. Returns [code]false[/code] when the joint is driven by [constant PARAM_ANGULAR_SPRING_EQUILIBRIUM_POINT] only. + + + + + + + Sets the target angular orientation as a body-space quaternion describing the desired orientation of body B relative to body A. Replaces any previously set target and supersedes [constant PARAM_ANGULAR_SPRING_EQUILIBRIUM_POINT] until [method clear_angular_target_rotation] is called. + [b]Note:[/b] Only for Jolt backend. Other 3D physics backends may ignore this value. + + diff --git a/doc/classes/PhysicsServer3D.xml b/doc/classes/PhysicsServer3D.xml index 54eb85f49d..37024d1405 100644 --- a/doc/classes/PhysicsServer3D.xml +++ b/doc/classes/PhysicsServer3D.xml @@ -759,6 +759,14 @@ Destroys any of the objects created by PhysicsServer3D. If the [RID] passed is not one of the objects that can be created by PhysicsServer3D, an error will be sent to the console. + + + + + Returns the target angular orientation of a generic 6DOF joint as a body-space quaternion describing the desired orientation of body B relative to body A. + [b]Note:[/b] Only for Jolt backend. Other 3D physics backends return the identity quaternion. + + @@ -777,6 +785,15 @@ Returns the value of a generic 6DOF joint parameter. + + + + + + Sets the target angular orientation of a generic 6DOF joint as a body-space quaternion describing the desired orientation of body B relative to body A. + [b]Note:[/b] Only for Jolt backend. Other 3D physics backends may ignore this value. + + diff --git a/doc/classes/PhysicsServer3DExtension.xml b/doc/classes/PhysicsServer3DExtension.xml index 806173848e..e5c2e59864 100644 --- a/doc/classes/PhysicsServer3DExtension.xml +++ b/doc/classes/PhysicsServer3DExtension.xml @@ -680,6 +680,12 @@ + + + + + + @@ -696,6 +702,13 @@ + + + + + + + diff --git a/modules/godot_physics_3d/godot_physics_server_3d.cpp b/modules/godot_physics_3d/godot_physics_server_3d.cpp index 66ca5ca223..05e4813b77 100644 --- a/modules/godot_physics_3d/godot_physics_server_3d.cpp +++ b/modules/godot_physics_3d/godot_physics_server_3d.cpp @@ -1596,6 +1596,25 @@ bool GodotPhysicsServer3D::generic_6dof_joint_get_flag(RID p_joint, Vector3::Axi return generic_6dof_joint->get_flag(p_axis, p_flag); } +void GodotPhysicsServer3D::generic_6dof_joint_set_angular_target_rotation(RID p_joint, const Quaternion &p_target_rotation) { + ERR_FAIL_COND_MSG(!p_target_rotation.is_finite() || p_target_rotation.length_squared() <= CMP_EPSILON, "Angular target rotation must be a finite, non-zero quaternion."); + + GodotJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL(joint); + ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_6DOF); + WARN_PRINT_ONCE("Quaternion angular target rotations for Generic6DOFJoint3D are only supported by Jolt Physics. This value will be ignored by GodotPhysics3D."); +} + +Quaternion GodotPhysicsServer3D::generic_6dof_joint_get_angular_target_rotation(RID p_joint) const { + const GodotJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL_V(joint, Quaternion()); + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_6DOF, Quaternion()); + + // GodotPhysics3D does not apply angular spring equilibrium points, so there is no body-space target to return. + WARN_PRINT_ONCE("Quaternion angular target rotations for Generic6DOFJoint3D are only supported by Jolt Physics. GodotPhysics3D returns the identity quaternion."); + return Quaternion(); +} + void GodotPhysicsServer3D::free_rid(RID p_rid) { _update_shapes(); //just in case diff --git a/modules/godot_physics_3d/godot_physics_server_3d.h b/modules/godot_physics_3d/godot_physics_server_3d.h index 417bfd26a7..052019f33c 100644 --- a/modules/godot_physics_3d/godot_physics_server_3d.h +++ b/modules/godot_physics_3d/godot_physics_server_3d.h @@ -361,6 +361,9 @@ public: virtual void generic_6dof_joint_set_flag(RID p_joint, Vector3::Axis, G6DOFJointAxisFlag p_flag, bool p_enable) override; virtual bool generic_6dof_joint_get_flag(RID p_joint, Vector3::Axis, G6DOFJointAxisFlag p_flag) const override; + virtual void generic_6dof_joint_set_angular_target_rotation(RID p_joint, const Quaternion &p_target_rotation) override; + virtual Quaternion generic_6dof_joint_get_angular_target_rotation(RID p_joint) const override; + virtual JointType joint_get_type(RID p_joint) const override; virtual void joint_set_solver_priority(RID p_joint, int p_priority) override; diff --git a/modules/jolt_physics/joints/jolt_generic_6dof_joint_3d.cpp b/modules/jolt_physics/joints/jolt_generic_6dof_joint_3d.cpp index 765e054ce1..e0707cf409 100644 --- a/modules/jolt_physics/joints/jolt_generic_6dof_joint_3d.cpp +++ b/modules/jolt_physics/joints/jolt_generic_6dof_joint_3d.cpp @@ -208,6 +208,20 @@ void JoltGeneric6DOFJoint3D::_update_spring_equilibrium(int p_axis) { } } +void JoltGeneric6DOFJoint3D::_update_angular_target_rotation() { + if (!has_angular_target_rotation) { + return; + } + + JPH::SixDOFConstraint *constraint = static_cast(jolt_ref.GetPtr()); + if (unlikely(constraint == nullptr)) { + return; + } + + const JPH::Quat target_orientation = to_jolt(angular_target_rotation); + constraint->SetTargetOrientationBS(target_orientation); +} + void JoltGeneric6DOFJoint3D::_limits_changed() { rebuild(); _wake_up_bodies(); @@ -435,6 +449,7 @@ void JoltGeneric6DOFJoint3D::set_param(Axis p_axis, Param p_param, double p_valu } break; case JoltPhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_EQUILIBRIUM_POINT: { spring_equilibrium[axis_ang] = p_value; + has_angular_target_rotation = false; _spring_equilibrium_changed(axis_ang); } break; default: { @@ -507,6 +522,36 @@ void JoltGeneric6DOFJoint3D::set_flag(Axis p_axis, Flag p_flag, bool p_enabled) } } +void JoltGeneric6DOFJoint3D::set_angular_target_rotation(const Quaternion &p_target_rotation) { + ERR_FAIL_COND_MSG(!p_target_rotation.is_finite() || p_target_rotation.length_squared() <= CMP_EPSILON, "Angular target rotation must be a finite, non-zero quaternion."); + + angular_target_rotation = p_target_rotation.normalized(); + has_angular_target_rotation = true; + _update_angular_target_rotation(); + _wake_up_bodies(); +} + +Quaternion JoltGeneric6DOFJoint3D::get_angular_target_rotation() const { + if (has_angular_target_rotation) { + return angular_target_rotation; + } + + const JPH::SixDOFConstraint *constraint = static_cast(jolt_ref.GetPtr()); + if (unlikely(constraint == nullptr)) { + return Quaternion(); + } + + const Basis target_orientation_cs = Basis::from_euler( + Vector3(static_cast(-spring_equilibrium[AXIS_ANGULAR_X]), + static_cast(-spring_equilibrium[AXIS_ANGULAR_Y]), + static_cast(-spring_equilibrium[AXIS_ANGULAR_Z])), + EulerOrder::ZYX); + const Basis constraint_to_body_1 = to_godot(constraint->GetConstraintToBody1Matrix()).basis; + const Basis constraint_to_body_2 = to_godot(constraint->GetConstraintToBody2Matrix()).basis; + + return (constraint_to_body_1 * target_orientation_cs * constraint_to_body_2.inverse()).get_quaternion(); +} + double JoltGeneric6DOFJoint3D::get_jolt_param(Axis p_axis, JoltParam p_param) const { const int axis_lin = AXES_LINEAR + (int)p_axis; const int axis_ang = AXES_ANGULAR + (int)p_axis; @@ -683,4 +728,6 @@ void JoltGeneric6DOFJoint3D::rebuild() { _update_spring_parameters(axis); _update_spring_equilibrium(axis); } + + _update_angular_target_rotation(); } diff --git a/modules/jolt_physics/joints/jolt_generic_6dof_joint_3d.h b/modules/jolt_physics/joints/jolt_generic_6dof_joint_3d.h index 56e611962a..04612c4951 100644 --- a/modules/jolt_physics/joints/jolt_generic_6dof_joint_3d.h +++ b/modules/jolt_physics/joints/jolt_generic_6dof_joint_3d.h @@ -72,6 +72,9 @@ class JoltGeneric6DOFJoint3D final : public JoltJoint3D { double spring_equilibrium[AXIS_COUNT] = {}; double spring_limit[AXIS_COUNT] = { FLT_MAX, FLT_MAX, FLT_MAX, FLT_MAX, FLT_MAX, FLT_MAX }; + Quaternion angular_target_rotation; + bool has_angular_target_rotation = false; + bool limit_enabled[AXIS_COUNT] = {}; bool limit_spring_enabled[AXIS_COUNT] = {}; @@ -89,6 +92,7 @@ class JoltGeneric6DOFJoint3D final : public JoltJoint3D { void _update_motor_limit(int p_axis); void _update_spring_parameters(int p_axis); void _update_spring_equilibrium(int p_axis); + void _update_angular_target_rotation(); void _limits_changed(); void _limit_spring_parameters_changed(int p_axis); @@ -111,6 +115,9 @@ public: bool get_flag(Axis p_axis, Flag p_flag) const; void set_flag(Axis p_axis, Flag p_flag, bool p_enabled); + void set_angular_target_rotation(const Quaternion &p_target_rotation); + Quaternion get_angular_target_rotation() const; + double get_jolt_param(Axis p_axis, JoltParam p_param) const; void set_jolt_param(Axis p_axis, JoltParam p_param, double p_value); diff --git a/modules/jolt_physics/jolt_physics_server_3d.cpp b/modules/jolt_physics/jolt_physics_server_3d.cpp index 3b68594b27..c2460dc4f1 100644 --- a/modules/jolt_physics/jolt_physics_server_3d.cpp +++ b/modules/jolt_physics/jolt_physics_server_3d.cpp @@ -1546,6 +1546,26 @@ bool JoltPhysicsServer3D::generic_6dof_joint_get_flag(RID p_joint, Vector3::Axis return g6dof_joint->get_flag(p_axis, p_flag); } +void JoltPhysicsServer3D::generic_6dof_joint_set_angular_target_rotation(RID p_joint, const Quaternion &p_target_rotation) { + JoltJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL(joint); + + ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_6DOF); + JoltGeneric6DOFJoint3D *g6dof_joint = static_cast(joint); + + g6dof_joint->set_angular_target_rotation(p_target_rotation); +} + +Quaternion JoltPhysicsServer3D::generic_6dof_joint_get_angular_target_rotation(RID p_joint) const { + const JoltJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL_V(joint, Quaternion()); + + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_6DOF, Quaternion()); + const JoltGeneric6DOFJoint3D *g6dof_joint = static_cast(joint); + + return g6dof_joint->get_angular_target_rotation(); +} + PhysicsServer3D::JointType JoltPhysicsServer3D::joint_get_type(RID p_joint) const { const JoltJoint3D *joint = joint_owner.get_or_null(p_joint); ERR_FAIL_NULL_V(joint, JOINT_TYPE_PIN); diff --git a/modules/jolt_physics/jolt_physics_server_3d.h b/modules/jolt_physics/jolt_physics_server_3d.h index a8db19b899..1ab95b796b 100644 --- a/modules/jolt_physics/jolt_physics_server_3d.h +++ b/modules/jolt_physics/jolt_physics_server_3d.h @@ -405,6 +405,9 @@ public: virtual void generic_6dof_joint_set_flag(RID p_joint, Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisFlag p_flag, bool p_enable) override; virtual bool generic_6dof_joint_get_flag(RID p_joint, Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisFlag p_flag) const override; + virtual void generic_6dof_joint_set_angular_target_rotation(RID p_joint, const Quaternion &p_target_rotation) override; + virtual Quaternion generic_6dof_joint_get_angular_target_rotation(RID p_joint) const override; + virtual PhysicsServer3D::JointType joint_get_type(RID p_joint) const override; virtual void joint_set_solver_priority(RID p_joint, int p_priority) override; diff --git a/modules/jolt_physics/misc/jolt_type_conversions.h b/modules/jolt_physics/misc/jolt_type_conversions.h index 52bf9a734e..0e5267456b 100644 --- a/modules/jolt_physics/misc/jolt_type_conversions.h +++ b/modules/jolt_physics/misc/jolt_type_conversions.h @@ -95,9 +95,12 @@ _FORCE_INLINE_ JPH::Vec3 to_jolt(const Vector3 &p_vec) { return JPH::Vec3((float)p_vec.x, (float)p_vec.y, (float)p_vec.z); } +_FORCE_INLINE_ JPH::Quat to_jolt(const Quaternion &p_quat) { + return JPH::Quat((float)p_quat.x, (float)p_quat.y, (float)p_quat.z, (float)p_quat.w); +} + _FORCE_INLINE_ JPH::Quat to_jolt(const Basis &p_basis) { - const Quaternion quat = p_basis.get_quaternion().normalized(); - return JPH::Quat((float)quat.x, (float)quat.y, (float)quat.z, (float)quat.w); + return to_jolt(p_basis.get_quaternion().normalized()); } _FORCE_INLINE_ JPH::Mat44 to_jolt(const Transform3D &p_transform) { diff --git a/scene/3d/physics/joints/generic_6dof_joint_3d.cpp b/scene/3d/physics/joints/generic_6dof_joint_3d.cpp index 87774956e9..475cfb3fcc 100644 --- a/scene/3d/physics/joints/generic_6dof_joint_3d.cpp +++ b/scene/3d/physics/joints/generic_6dof_joint_3d.cpp @@ -51,6 +51,11 @@ void Generic6DOFJoint3D::_bind_methods() { ClassDB::bind_method(D_METHOD("set_flag_z", "flag", "value"), &Generic6DOFJoint3D::set_flag_z); ClassDB::bind_method(D_METHOD("get_flag_z", "flag"), &Generic6DOFJoint3D::get_flag_z); + ClassDB::bind_method(D_METHOD("set_angular_target_rotation", "target_rotation"), &Generic6DOFJoint3D::set_angular_target_rotation); + ClassDB::bind_method(D_METHOD("get_angular_target_rotation"), &Generic6DOFJoint3D::get_angular_target_rotation); + ClassDB::bind_method(D_METHOD("has_target_rotation"), &Generic6DOFJoint3D::has_target_rotation); + ClassDB::bind_method(D_METHOD("clear_angular_target_rotation"), &Generic6DOFJoint3D::clear_angular_target_rotation); + ADD_GROUP("Linear Limit", "linear_limit_"); ADD_PROPERTYI(PropertyInfo(Variant::BOOL, "linear_limit_x/enabled"), "set_flag_x", "get_flag_x", FLAG_ENABLE_LINEAR_LIMIT); @@ -201,6 +206,9 @@ void Generic6DOFJoint3D::_bind_methods() { void Generic6DOFJoint3D::set_param_x(Param p_param, real_t p_value) { ERR_FAIL_INDEX(p_param, PARAM_MAX); params_x[p_param] = p_value; + if (p_param == PARAM_ANGULAR_SPRING_EQUILIBRIUM_POINT) { + has_angular_target_rotation = false; + } if (is_configured()) { PhysicsServer3D::get_singleton()->generic_6dof_joint_set_param(get_rid(), Vector3::AXIS_X, PhysicsServer3D::G6DOFJointAxisParam(p_param), p_value); } @@ -216,6 +224,9 @@ real_t Generic6DOFJoint3D::get_param_x(Param p_param) const { void Generic6DOFJoint3D::set_param_y(Param p_param, real_t p_value) { ERR_FAIL_INDEX(p_param, PARAM_MAX); params_y[p_param] = p_value; + if (p_param == PARAM_ANGULAR_SPRING_EQUILIBRIUM_POINT) { + has_angular_target_rotation = false; + } if (is_configured()) { PhysicsServer3D::get_singleton()->generic_6dof_joint_set_param(get_rid(), Vector3::AXIS_Y, PhysicsServer3D::G6DOFJointAxisParam(p_param), p_value); } @@ -230,6 +241,9 @@ real_t Generic6DOFJoint3D::get_param_y(Param p_param) const { void Generic6DOFJoint3D::set_param_z(Param p_param, real_t p_value) { ERR_FAIL_INDEX(p_param, PARAM_MAX); params_z[p_param] = p_value; + if (p_param == PARAM_ANGULAR_SPRING_EQUILIBRIUM_POINT) { + has_angular_target_rotation = false; + } if (is_configured()) { PhysicsServer3D::get_singleton()->generic_6dof_joint_set_param(get_rid(), Vector3::AXIS_Z, PhysicsServer3D::G6DOFJointAxisParam(p_param), p_value); } @@ -283,6 +297,58 @@ bool Generic6DOFJoint3D::get_flag_z(Flag p_flag) const { return flags_z[p_flag]; } +bool Generic6DOFJoint3D::_is_valid_angular_target_rotation(const Quaternion &p_target_rotation) { + return p_target_rotation.is_finite() && p_target_rotation.length_squared() > CMP_EPSILON; +} + +void Generic6DOFJoint3D::set_angular_target_rotation(const Quaternion &p_target_rotation) { + ERR_FAIL_COND_MSG(!_is_valid_angular_target_rotation(p_target_rotation), "Angular target rotation must be a finite, non-zero quaternion."); + + angular_target_rotation = p_target_rotation.normalized(); + has_angular_target_rotation = true; + + if (!is_configured()) { + return; + } + + PhysicsServer3D::get_singleton()->generic_6dof_joint_set_angular_target_rotation(get_rid(), angular_target_rotation); +} + +Quaternion Generic6DOFJoint3D::get_angular_target_rotation() const { + if (has_angular_target_rotation) { + return angular_target_rotation; + } + + if (is_configured()) { + return PhysicsServer3D::get_singleton()->generic_6dof_joint_get_angular_target_rotation(get_rid()); + } + + // Equilibrium point is in constraint space; body-space conversion requires the constraint frame. + ERR_PRINT("Cannot derive a body-space angular target rotation from Generic6DOFJoint3D equilibrium points before the joint is configured. Returning the identity quaternion."); + return Quaternion(); +} + +bool Generic6DOFJoint3D::has_target_rotation() const { + return has_angular_target_rotation; +} + +void Generic6DOFJoint3D::clear_angular_target_rotation() { + if (!has_angular_target_rotation) { + return; + } + has_angular_target_rotation = false; + angular_target_rotation = Quaternion(); + + if (!is_configured()) { + return; + } + + PhysicsServer3D *server = PhysicsServer3D::get_singleton(); + server->generic_6dof_joint_set_param(get_rid(), Vector3::AXIS_X, PhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_EQUILIBRIUM_POINT, params_x[PARAM_ANGULAR_SPRING_EQUILIBRIUM_POINT]); + server->generic_6dof_joint_set_param(get_rid(), Vector3::AXIS_Y, PhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_EQUILIBRIUM_POINT, params_y[PARAM_ANGULAR_SPRING_EQUILIBRIUM_POINT]); + server->generic_6dof_joint_set_param(get_rid(), Vector3::AXIS_Z, PhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_EQUILIBRIUM_POINT, params_z[PARAM_ANGULAR_SPRING_EQUILIBRIUM_POINT]); +} + void Generic6DOFJoint3D::_configure_joint(RID p_joint, PhysicsBody3D *body_a, PhysicsBody3D *body_b) { Transform3D gt = get_global_transform(); //Vector3 cone_twistpos = gt.origin; @@ -312,6 +378,10 @@ void Generic6DOFJoint3D::_configure_joint(RID p_joint, PhysicsBody3D *body_a, Ph PhysicsServer3D::get_singleton()->generic_6dof_joint_set_flag(p_joint, Vector3::AXIS_Y, PhysicsServer3D::G6DOFJointAxisFlag(i), flags_y[i]); PhysicsServer3D::get_singleton()->generic_6dof_joint_set_flag(p_joint, Vector3::AXIS_Z, PhysicsServer3D::G6DOFJointAxisFlag(i), flags_z[i]); } + + if (has_angular_target_rotation) { + PhysicsServer3D::get_singleton()->generic_6dof_joint_set_angular_target_rotation(p_joint, angular_target_rotation); + } } Generic6DOFJoint3D::Generic6DOFJoint3D() { diff --git a/scene/3d/physics/joints/generic_6dof_joint_3d.h b/scene/3d/physics/joints/generic_6dof_joint_3d.h index 85f0b604a7..02de0b86e3 100644 --- a/scene/3d/physics/joints/generic_6dof_joint_3d.h +++ b/scene/3d/physics/joints/generic_6dof_joint_3d.h @@ -79,10 +79,14 @@ protected: bool flags_y[FLAG_MAX]; real_t params_z[PARAM_MAX]; bool flags_z[FLAG_MAX]; + Quaternion angular_target_rotation; + bool has_angular_target_rotation = false; virtual void _configure_joint(RID p_joint, PhysicsBody3D *body_a, PhysicsBody3D *body_b) override; static void _bind_methods(); + static bool _is_valid_angular_target_rotation(const Quaternion &p_target_rotation); + public: void set_param_x(Param p_param, real_t p_value); real_t get_param_x(Param p_param) const; @@ -102,6 +106,11 @@ public: void set_flag_z(Flag p_flag, bool p_enabled); bool get_flag_z(Flag p_flag) const; + void set_angular_target_rotation(const Quaternion &p_target_rotation); + Quaternion get_angular_target_rotation() const; + bool has_target_rotation() const; + void clear_angular_target_rotation(); + Generic6DOFJoint3D(); }; diff --git a/servers/physics_3d/physics_server_3d.cpp b/servers/physics_3d/physics_server_3d.cpp index 67dee00fe2..f2165e05a8 100644 --- a/servers/physics_3d/physics_server_3d.cpp +++ b/servers/physics_3d/physics_server_3d.cpp @@ -1044,6 +1044,9 @@ void PhysicsServer3D::_bind_methods() { ClassDB::bind_method(D_METHOD("generic_6dof_joint_set_flag", "joint", "axis", "flag", "enable"), &PhysicsServer3D::generic_6dof_joint_set_flag); ClassDB::bind_method(D_METHOD("generic_6dof_joint_get_flag", "joint", "axis", "flag"), &PhysicsServer3D::generic_6dof_joint_get_flag); + ClassDB::bind_method(D_METHOD("generic_6dof_joint_set_angular_target_rotation", "joint", "target_rotation"), &PhysicsServer3D::generic_6dof_joint_set_angular_target_rotation); + ClassDB::bind_method(D_METHOD("generic_6dof_joint_get_angular_target_rotation", "joint"), &PhysicsServer3D::generic_6dof_joint_get_angular_target_rotation); + ClassDB::bind_method(D_METHOD("free_rid", "rid"), &PhysicsServer3D::free_rid); ClassDB::bind_method(D_METHOD("set_active", "active"), &PhysicsServer3D::set_active); diff --git a/servers/physics_3d/physics_server_3d.h b/servers/physics_3d/physics_server_3d.h index 3738c1ee70..572ce02ae2 100644 --- a/servers/physics_3d/physics_server_3d.h +++ b/servers/physics_3d/physics_server_3d.h @@ -794,6 +794,9 @@ public: virtual void generic_6dof_joint_set_flag(RID p_joint, Vector3::Axis, G6DOFJointAxisFlag p_flag, bool p_enable) = 0; virtual bool generic_6dof_joint_get_flag(RID p_joint, Vector3::Axis, G6DOFJointAxisFlag p_flag) const = 0; + virtual void generic_6dof_joint_set_angular_target_rotation(RID p_joint, const Quaternion &p_target_rotation) = 0; + virtual Quaternion generic_6dof_joint_get_angular_target_rotation(RID p_joint) const = 0; + /* QUERY API */ enum AreaBodyStatus { diff --git a/servers/physics_3d/physics_server_3d_dummy.h b/servers/physics_3d/physics_server_3d_dummy.h index d788423ba5..7e4186191d 100644 --- a/servers/physics_3d/physics_server_3d_dummy.h +++ b/servers/physics_3d/physics_server_3d_dummy.h @@ -423,6 +423,9 @@ public: virtual void generic_6dof_joint_set_flag(RID p_joint, Vector3::Axis, G6DOFJointAxisFlag p_flag, bool p_enable) override {} virtual bool generic_6dof_joint_get_flag(RID p_joint, Vector3::Axis, G6DOFJointAxisFlag p_flag) const override { return false; } + virtual void generic_6dof_joint_set_angular_target_rotation(RID p_joint, const Quaternion &p_target_rotation) override {} + virtual Quaternion generic_6dof_joint_get_angular_target_rotation(RID p_joint) const override { return Quaternion(); } + /* MISC */ virtual void free_rid(RID p_rid) override {} diff --git a/servers/physics_3d/physics_server_3d_extension.cpp b/servers/physics_3d/physics_server_3d_extension.cpp index 37bf961fba..6d5e9f4ede 100644 --- a/servers/physics_3d/physics_server_3d_extension.cpp +++ b/servers/physics_3d/physics_server_3d_extension.cpp @@ -417,6 +417,9 @@ void PhysicsServer3DExtension::_bind_methods() { GDVIRTUAL_BIND(_generic_6dof_joint_set_flag, "joint", "axis", "flag", "enable"); GDVIRTUAL_BIND(_generic_6dof_joint_get_flag, "joint", "axis", "flag"); + GDVIRTUAL_BIND(_generic_6dof_joint_set_angular_target_rotation, "joint", "target_rotation"); + GDVIRTUAL_BIND(_generic_6dof_joint_get_angular_target_rotation, "joint"); + GDVIRTUAL_BIND(_joint_get_type, "joint"); GDVIRTUAL_BIND(_joint_set_solver_priority, "joint", "priority"); diff --git a/servers/physics_3d/physics_server_3d_extension.h b/servers/physics_3d/physics_server_3d_extension.h index 086208e74d..7473f7ad9d 100644 --- a/servers/physics_3d/physics_server_3d_extension.h +++ b/servers/physics_3d/physics_server_3d_extension.h @@ -522,6 +522,9 @@ public: EXBIND4(generic_6dof_joint_set_flag, RID, Vector3::Axis, G6DOFJointAxisFlag, bool) EXBIND3RC(bool, generic_6dof_joint_get_flag, RID, Vector3::Axis, G6DOFJointAxisFlag) + EXBIND2(generic_6dof_joint_set_angular_target_rotation, RID, const Quaternion &) + EXBIND1RC(Quaternion, generic_6dof_joint_get_angular_target_rotation, RID) + EXBIND1RC(JointType, joint_get_type, RID) EXBIND2(joint_set_solver_priority, RID, int) diff --git a/servers/physics_3d/physics_server_3d_wrap_mt.h b/servers/physics_3d/physics_server_3d_wrap_mt.h index 50aa2d240e..0958a95f2e 100644 --- a/servers/physics_3d/physics_server_3d_wrap_mt.h +++ b/servers/physics_3d/physics_server_3d_wrap_mt.h @@ -383,6 +383,9 @@ public: FUNC4(generic_6dof_joint_set_flag, RID, Vector3::Axis, G6DOFJointAxisFlag, bool) FUNC3RC(bool, generic_6dof_joint_get_flag, RID, Vector3::Axis, G6DOFJointAxisFlag) + FUNC2(generic_6dof_joint_set_angular_target_rotation, RID, const Quaternion &) + FUNC1RC(Quaternion, generic_6dof_joint_get_angular_target_rotation, RID) + FUNC1RC(JointType, joint_get_type, RID); FUNC2(joint_set_solver_priority, RID, int); diff --git a/tests/scene/test_generic_6dof_joint_3d.cpp b/tests/scene/test_generic_6dof_joint_3d.cpp new file mode 100644 index 0000000000..166e267560 --- /dev/null +++ b/tests/scene/test_generic_6dof_joint_3d.cpp @@ -0,0 +1,111 @@ +/**************************************************************************/ +/* test_generic_6dof_joint_3d.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#include "tests/test_macros.h" + +TEST_FORCE_LINK(test_generic_6dof_joint_3d) + +#ifndef PHYSICS_3D_DISABLED + +#include "scene/3d/physics/joints/generic_6dof_joint_3d.h" + +namespace TestGeneric6DOFJoint3D { + +TEST_CASE("[SceneTree][Generic6DOFJoint3D] Quaternion angular target is normalized and stored") { + Generic6DOFJoint3D *joint = memnew(Generic6DOFJoint3D); + + const Quaternion target_rotation(Vector3(0, 1, 0), 0.5); + joint->set_angular_target_rotation(target_rotation * 2.0); + + CHECK(joint->get_angular_target_rotation().is_normalized()); + CHECK(joint->get_angular_target_rotation().is_equal_approx(target_rotation)); + CHECK(joint->has_target_rotation()); + + memdelete(joint); +} + +TEST_CASE("[SceneTree][Generic6DOFJoint3D] Euler equilibrium setter clears stored quaternion target") { + Generic6DOFJoint3D *joint = memnew(Generic6DOFJoint3D); + + joint->set_angular_target_rotation(Quaternion(Vector3(0, 0, 1), 0.25)); + CHECK(joint->has_target_rotation()); + + joint->set_param_x(Generic6DOFJoint3D::PARAM_ANGULAR_SPRING_EQUILIBRIUM_POINT, 0.1); + CHECK_FALSE(joint->has_target_rotation()); + + memdelete(joint); +} + +TEST_CASE("[SceneTree][Generic6DOFJoint3D] clear_angular_target_rotation resets state") { + Generic6DOFJoint3D *joint = memnew(Generic6DOFJoint3D); + + joint->set_angular_target_rotation(Quaternion(Vector3(1, 0, 0), 0.5)); + CHECK(joint->has_target_rotation()); + + joint->clear_angular_target_rotation(); + CHECK_FALSE(joint->has_target_rotation()); + ERR_PRINT_OFF; + CHECK(joint->get_angular_target_rotation().is_equal_approx(Quaternion())); + ERR_PRINT_ON; + + memdelete(joint); +} + +TEST_CASE("[SceneTree][Generic6DOFJoint3D] Getter returns identity before configuration when no explicit target is set") { + Generic6DOFJoint3D *joint = memnew(Generic6DOFJoint3D); + + joint->set_param_x(Generic6DOFJoint3D::PARAM_ANGULAR_SPRING_EQUILIBRIUM_POINT, 0.3); + joint->set_param_y(Generic6DOFJoint3D::PARAM_ANGULAR_SPRING_EQUILIBRIUM_POINT, -0.2); + joint->set_param_z(Generic6DOFJoint3D::PARAM_ANGULAR_SPRING_EQUILIBRIUM_POINT, 0.4); + + CHECK_FALSE(joint->has_target_rotation()); + ERR_PRINT_OFF; + CHECK(joint->get_angular_target_rotation().is_equal_approx(Quaternion())); + ERR_PRINT_ON; + + memdelete(joint); +} + +TEST_CASE("[SceneTree][Generic6DOFJoint3D] Explicit quaternion target shadows Euler equilibrium derivation") { + Generic6DOFJoint3D *joint = memnew(Generic6DOFJoint3D); + + joint->set_param_x(Generic6DOFJoint3D::PARAM_ANGULAR_SPRING_EQUILIBRIUM_POINT, 0.3); + const Quaternion explicit_target(Vector3(0, 1, 0), 0.7); + joint->set_angular_target_rotation(explicit_target); + + CHECK(joint->has_target_rotation()); + CHECK(joint->get_angular_target_rotation().is_equal_approx(explicit_target)); + + memdelete(joint); +} + +} // namespace TestGeneric6DOFJoint3D + +#endif // PHYSICS_3D_DISABLED