diff --git a/doc/classes/CCDIK3D.xml b/doc/classes/CCDIK3D.xml
new file mode 100644
index 00000000000..5db4ef2e246
--- /dev/null
+++ b/doc/classes/CCDIK3D.xml
@@ -0,0 +1,13 @@
+
+
+
+ Rotation based cyclic coordinate descent inverse kinematics solver.
+
+
+ [CCDIK3D] is rotation based IK, enabling fast and effective tracking even with large joint rotations. It's especially suitable for chains with limitations, providing smoother and more stable target tracking compared to [FABRIK3D].
+ The resulting twist around the forward vector will always be kept from the previous pose.
+ [b]Note:[/b] When the target is close to the root, it can cause unnatural movement, including joint flips and oscillations.
+
+
+
+
diff --git a/doc/classes/ChainIK3D.xml b/doc/classes/ChainIK3D.xml
new file mode 100644
index 00000000000..a36886fdac9
--- /dev/null
+++ b/doc/classes/ChainIK3D.xml
@@ -0,0 +1,143 @@
+
+
+
+ A [SkeletonModifier3D] to apply inverse kinematics to bone chains containing an arbitrary number of bones.
+
+
+ Base class of [SkeletonModifier3D] that automatically generates a joint list from the bones between the root bone and the end bone.
+
+
+
+
+
+
+
+
+ Returns the end bone index of the bone chain.
+
+
+
+
+
+
+ Returns the tail direction of the end bone of the bone chain when [method is_end_bone_extended] is [code]true[/code].
+
+
+
+
+
+
+ Returns the end bone tail length of the bone chain when [method is_end_bone_extended] is [code]true[/code].
+
+
+
+
+
+
+ Returns the end bone name of the bone chain.
+
+
+
+
+
+
+
+ Returns the bone index at [param joint] in the bone chain's joint list.
+
+
+
+
+
+
+
+ Returns the bone name at [param joint] in the bone chain's joint list.
+
+
+
+
+
+
+ Returns the joint count of the bone chain's joint list.
+
+
+
+
+
+
+ Returns the root bone index of the bone chain.
+
+
+
+
+
+
+ Returns the root bone name of the bone chain.
+
+
+
+
+
+
+ Returns [code]true[/code] if the end bone is extended to have a tail.
+
+
+
+
+
+
+
+ Sets the end bone index of the bone chain.
+
+
+
+
+
+
+
+ Sets the end bone tail direction of the bone chain when [method is_end_bone_extended] is [code]true[/code].
+
+
+
+
+
+
+
+ Sets the end bone tail length of the bone chain when [method is_end_bone_extended] is [code]true[/code].
+
+
+
+
+
+
+
+ Sets the end bone name of the bone chain.
+ [b]Note:[/b] The end bone must be the root bone or a child of the root bone. If they are the same, the tail must be extended by [method set_extend_end_bone] to modify the bone.
+
+
+
+
+
+
+
+ If [param enabled] is [code]true[/code], the end bone is extended to have a tail.
+ The extended tail config is allocated to the last element in the joint list. In other words, if you set [param enabled] to [code]false[/code], the config of the last element in the joint list has no effect in the simulated result.
+
+
+
+
+
+
+
+ Sets the root bone index of the bone chain.
+
+
+
+
+
+
+
+ Sets the root bone name of the bone chain.
+
+
+
+
diff --git a/doc/classes/EditorSettings.xml b/doc/classes/EditorSettings.xml
index 82e8f01cf72..30746fbc7be 100644
--- a/doc/classes/EditorSettings.xml
+++ b/doc/classes/EditorSettings.xml
@@ -456,6 +456,9 @@
The 3D editor gizmo color for the [GridMap] grid.
+
+ The 3D editor gizmo color for the [IKModifier3D] guides.
+
The color override to use for 3D editor gizmos if the [Node3D] in question is part of an instantiated scene file (from the perspective of the current scene).
diff --git a/doc/classes/FABRIK3D.xml b/doc/classes/FABRIK3D.xml
new file mode 100644
index 00000000000..90cf990d9f5
--- /dev/null
+++ b/doc/classes/FABRIK3D.xml
@@ -0,0 +1,13 @@
+
+
+
+ Position based forward and backward reaching inverse kinematics solver.
+
+
+ [FABRIK3D] is position based IK, allowing precise and accurate tracking of targets. It's ideal for simple chains without limitations.
+ The resulting twist around the forward vector will always be kept from the previous pose.
+ [b]Note:[/b] When the target is close to the root, it tends to produce zig-zag patterns, resulting in unnatural visual movement.
+
+
+
+
diff --git a/doc/classes/IKModifier3D.xml b/doc/classes/IKModifier3D.xml
new file mode 100644
index 00000000000..0ad6bccb9cb
--- /dev/null
+++ b/doc/classes/IKModifier3D.xml
@@ -0,0 +1,39 @@
+
+
+
+ A node for inverse kinematics which may modify more than one bone.
+
+
+ Base class of [SkeletonModifier3D]s that has some joint lists and applies inverse kinematics. This class has some structs, enums, and helper methods which are useful to solve inverse kinematics.
+ [b]Note:[/b] The IK classes that extend this handle rotation only, with bone lengths cached. It means that a position movement between processed chains can cause unintended movement.
+
+
+
+
+
+
+
+ Clears all settings.
+
+
+
+
+
+ Returns the number of settings.
+
+
+
+
+
+ Resets a state with respect to the current bone pose.
+
+
+
+
+
+
+ Sets the number of settings.
+
+
+
+
diff --git a/doc/classes/IterateIK3D.xml b/doc/classes/IterateIK3D.xml
new file mode 100644
index 00000000000..76f7148f41a
--- /dev/null
+++ b/doc/classes/IterateIK3D.xml
@@ -0,0 +1,158 @@
+
+
+
+ A [SkeletonModifier3D] to approach the goal by repeating small rotations.
+
+
+ Base class of [SkeletonModifier3D] to approach the goal by repeating small rotations.
+ Each bone chain (setting) has one effector, which is processed in order of the setting list. You can set some limitations for each joint.
+
+
+
+
+
+
+
+
+
+ Returns the joint limitation at [param joint] in the bone chain's joint list.
+
+
+
+
+
+
+
+ Returns the joint limitation right axis at [param joint] in the bone chain's joint list.
+
+
+
+
+
+
+
+ Returns the joint limitation right axis vector at [param joint] in the bone chain's joint list.
+ If [method get_joint_limitation_right_axis] is [constant SkeletonModifier3D.SECONDARY_DIRECTION_NONE], this method returns [code]Vector3(0, 0, 0)[/code].
+
+
+
+
+
+
+
+ Returns the joint limitation rotation offset at [param joint] in the bone chain's joint list.
+ Rotation is done in the local space which is constructed by the bone direction (in general parent to child) as the +Y axis and [method get_joint_limitation_right_axis_vector] as the +X axis.
+ If the +X and +Y axes are not orthogonal, the +X axis is implicitly modified to make it orthogonal.
+ Also, if the length of [method get_joint_limitation_right_axis_vector] is zero, the space is created by rotating the bone rest using the shortest arc that rotates the +Y axis of the bone rest to match the bone direction.
+
+
+
+
+
+
+
+ Returns the rotation axis at [param joint] in the bone chain's joint list.
+
+
+
+
+
+
+
+ Returns the rotation axis vector for the specified joint in the bone chain. This vector represents the axis around which the joint can rotate. It is determined based on the rotation axis set for the joint.
+ If [method get_joint_rotation_axis] is [constant SkeletonModifier3D.ROTATION_AXIS_ALL], this method returns [code]Vector3(0, 0, 0)[/code].
+
+
+
+
+
+
+ Returns the target node that the end bone is trying to reach.
+
+
+
+
+
+
+
+
+ Sets the joint limitation at [param joint] in the bone chain's joint list.
+
+
+
+
+
+
+
+
+ Sets the joint limitation right axis at [param joint] in the bone chain's joint list.
+
+
+
+
+
+
+
+
+ Sets the optional joint limitation right axis vector at [param joint] in the bone chain's joint list.
+
+
+
+
+
+
+
+
+ Sets the joint limitation rotation offset at [param joint] in the bone chain's joint list.
+ Rotation is done in the local space which is constructed by the bone direction (in general parent to child) as the +Y axis and [method get_joint_limitation_right_axis_vector] as the +X axis.
+ If the +X and +Y axes are not orthogonal, the +X axis is implicitly modified to make it orthogonal.
+ Also, if the length of [method get_joint_limitation_right_axis_vector] is zero, the space is created by rotating the bone rest using the shortest arc that rotates the +Y axis of the bone rest to match the bone direction.
+
+
+
+
+
+
+
+
+ Sets the rotation axis at [param joint] in the bone chain's joint list.
+ The axes are based on the [method Skeleton3D.get_bone_rest]'s space, if [param axis] is [constant SkeletonModifier3D.ROTATION_AXIS_CUSTOM], you can specify any axis.
+ [b]Note:[/b] The rotation axis and the forward vector shouldn't be colinear to avoid unintended rotation since [ChainIK3D] does not factor in twisting forces.
+
+
+
+
+
+
+
+
+ Sets the rotation axis vector for the specified joint in the bone chain.
+ This vector is normalized by an internal process and represents the axis around which the bone chain can rotate.
+ If the vector length is [code]0[/code], it is considered synonymous with [constant SkeletonModifier3D.ROTATION_AXIS_ALL].
+
+
+
+
+
+
+
+ Sets the target node that the end bone is trying to reach.
+
+
+
+
+
+ The maximum amount each bone can rotate in a single iteration.
+ [b]Note:[/b] This limitation is applied during each iteration. For example, if [member max_iterations] is [code]4[/code] and [member angular_delta_limit] is [code]5[/code] degrees, the maximum rotation possible in a single frame is [code]20[/code] degrees.
+
+
+ The number of iteration loops used by the IK solver to produce more accurate results.
+
+
+ The minimum distance between the end bone and the target. If the distance is below this value, the IK solver stops any further iterations.
+
+
+ The number of settings.
+
+
+
diff --git a/doc/classes/JacobianIK3D.xml b/doc/classes/JacobianIK3D.xml
new file mode 100644
index 00000000000..6bbece2de1a
--- /dev/null
+++ b/doc/classes/JacobianIK3D.xml
@@ -0,0 +1,13 @@
+
+
+
+ Pseudo inverse Jacobian matrix based inverse kinematics solver.
+
+
+ [JacobianIK3D] calculates rotations for all joints simultaneously, producing natural and smooth movement. It is particularly suited for biological animations.
+ The resulting twist around the forward vector will always be kept from the previous pose.
+ [b]Note:[/b] It converges more slowly than other IK solvers, leading to gentler and less immediate tracking of targets.
+
+
+
+
diff --git a/doc/classes/JointLimitation3D.xml b/doc/classes/JointLimitation3D.xml
new file mode 100644
index 00000000000..98a08ac60f7
--- /dev/null
+++ b/doc/classes/JointLimitation3D.xml
@@ -0,0 +1,11 @@
+
+
+
+ A base class of the limitation that interacts with [ChainIK3D].
+
+
+ The limitation is attached to each joint and limits the rotation of the bone.
+
+
+
+
diff --git a/doc/classes/JointLimitationCone3D.xml b/doc/classes/JointLimitationCone3D.xml
new file mode 100644
index 00000000000..135135f50dd
--- /dev/null
+++ b/doc/classes/JointLimitationCone3D.xml
@@ -0,0 +1,17 @@
+
+
+
+ A cone shape limitation that interacts with [ChainIK3D].
+
+
+ A cone shape limitation that interacts with [ChainIK3D].
+
+
+
+
+
+ The size of the hole made by the cone.
+ [code]0[/code] is no hole, [code]0.5[/code] makes a hemisphere, and [code]1.0[/code] makes a sphere (no limitation).
+
+
+
diff --git a/doc/classes/SkeletonModifier3D.xml b/doc/classes/SkeletonModifier3D.xml
index 2e633815a52..621c59fcd83 100644
--- a/doc/classes/SkeletonModifier3D.xml
+++ b/doc/classes/SkeletonModifier3D.xml
@@ -85,5 +85,65 @@
Enumerated value for the -Z axis.
+
+ Enumerated value for the +X axis.
+
+
+ Enumerated value for the -X axis.
+
+
+ Enumerated value for the +Y axis.
+
+
+ Enumerated value for the -Y axis.
+
+
+ Enumerated value for the +Z axis.
+
+
+ Enumerated value for the -Z axis.
+
+
+ Enumerated value for the axis from a parent bone to the child bone.
+
+
+ Enumerated value for the case when the axis is undefined.
+
+
+ Enumerated value for the +X axis.
+
+
+ Enumerated value for the -X axis.
+
+
+ Enumerated value for the +Y axis.
+
+
+ Enumerated value for the -Y axis.
+
+
+ Enumerated value for the +Z axis.
+
+
+ Enumerated value for the -Z axis.
+
+
+ Enumerated value for an optional axis.
+
+
+ Enumerated value for the rotation of the X axis.
+
+
+ Enumerated value for the rotation of the Y axis.
+
+
+ Enumerated value for the rotation of the Z axis.
+
+
+ Enumerated value for the unconstrained rotation.
+
+
+ Enumerated value for an optional rotation axis.
+
diff --git a/doc/classes/SplineIK3D.xml b/doc/classes/SplineIK3D.xml
new file mode 100644
index 00000000000..579bc9a211b
--- /dev/null
+++ b/doc/classes/SplineIK3D.xml
@@ -0,0 +1,86 @@
+
+
+
+ A [SkeletonModifier3D] for aligning bones along a [Path3D].
+
+
+ A [SkeletonModifier3D] for aligning bones along a [Path3D]. The smoothness of the fitting depends on the [member Curve3D.bake_interval].
+ If you want the [Path3D] to attach to a specific bone, it is recommended to place a [ModifierBoneTarget3D] before the [SplineIK3D] in the [SkeletonModifier3D] list (children of the [Skeleton3D]), and then place a [Path3D] as the [ModifierBoneTarget3D]'s child.
+ Bone twist is determined based on the [method Curve3D.get_point_tilt].
+ If the root bone joint and the start point of the [Curve3D] are separated, it assumes that there is a linear line segment between them. This means that the vector pointing toward the start point of the [Curve3D] takes precedence over the shortest intersection point along the [Curve3D].
+ If the end bone joint exceeds the path length, it is bent as close as possible to the end point of the [Curve3D].
+
+
+
+
+
+
+
+
+ Returns the node path of the [Path3D] which is describing the path.
+
+
+
+
+
+
+ Returns the tilt interpolation method used between the root bone and the start point of the [Curve3D] when they are apart. See also [method set_tilt_fade_in].
+
+
+
+
+
+
+ Returns the tilt interpolation method used between the end bone and the end point of the [Curve3D] when they are apart. See also [method set_tilt_fade_out].
+
+
+
+
+
+
+ Returns if the tilt property of the [Curve3D] affects the bone twist.
+
+
+
+
+
+
+
+ Sets the node path of the [Path3D] which is describing the path.
+
+
+
+
+
+
+
+ Sets if the tilt property of the [Curve3D] should affect the bone twist.
+
+
+
+
+
+
+
+ If [param size] is greater than [code]0[/code], the tilt is interpolated between [param size] start bones from the start point of the [Curve3D] when they are apart.
+ If [param size] is equal [code]0[/code], the tilts between the root bone head and the start point of the [Curve3D] are unified with a tilt of the start point of the [Curve3D].
+ If [param size] is less than [code]0[/code], the tilts between the root bone and the start point of the [Curve3D] are [code]0.0[/code].
+
+
+
+
+
+
+
+ If [param size] is greater than [code]0[/code], the tilt is interpolated between [param size] end bones from the end point of the [Curve3D] when they are apart.
+ If [param size] is equal [code]0[/code], the tilts between the end bone tail and the end point of the [Curve3D] are unified with a tilt of the end point of the [Curve3D].
+ If [param size] is less than [code]0[/code], the tilts between the end bone and the end point of the [Curve3D] are [code]0.0[/code].
+
+
+
+
+
+ The number of settings.
+
+
+
diff --git a/doc/classes/SpringBoneSimulator3D.xml b/doc/classes/SpringBoneSimulator3D.xml
index 235b0c10fb7..914ebfeddd4 100644
--- a/doc/classes/SpringBoneSimulator3D.xml
+++ b/doc/classes/SpringBoneSimulator3D.xml
@@ -107,17 +107,17 @@
-
+
- Returns the end bone's tail direction of the bone chain when [method is_end_bone_extended] is [code]true[/code].
+ Returns the tail direction of the end bone of the bone chain when [method is_end_bone_extended] is [code]true[/code].
- Returns the end bone's tail length of the bone chain when [method is_end_bone_extended] is [code]true[/code].
+ Returns the end bone tail length of the bone chain when [method is_end_bone_extended] is [code]true[/code].
@@ -219,7 +219,7 @@
-
+
@@ -232,7 +232,7 @@
Returns the rotation axis vector for the specified joint in the bone chain. This vector represents the axis around which the joint can rotate. It is determined based on the rotation axis set for the joint.
- If [method get_joint_rotation_axis] is [constant ROTATION_AXIS_ALL], this method returns [code]Vector3(0, 0, 0)[/code].
+ If [method get_joint_rotation_axis] is [constant SkeletonModifier3D.ROTATION_AXIS_ALL], this method returns [code]Vector3(0, 0, 0)[/code].
@@ -272,7 +272,7 @@
-
+
Returns the rotation axis of the bone chain.
@@ -283,7 +283,7 @@
Returns the rotation axis vector of the bone chain. This vector represents the axis around which the bone chain can rotate. It is determined based on the rotation axis set for the bone chain.
- If [method get_rotation_axis] is [constant ROTATION_AXIS_ALL], this method returns [code]Vector3(0, 0, 0)[/code].
+ If [method get_rotation_axis] is [constant SkeletonModifier3D.ROTATION_AXIS_ALL], this method returns [code]Vector3(0, 0, 0)[/code].
@@ -311,7 +311,7 @@
- Returns [code]true[/code] if the end bone is extended to have the tail.
+ Returns [code]true[/code] if the end bone is extended to have a tail.
@@ -410,7 +410,7 @@
-
+
Sets the end bone tail direction of the bone chain when [method is_end_bone_extended] is [code]true[/code].
@@ -454,9 +454,8 @@
- If [param enabled] is [code]true[/code], the end bone is extended to have the tail.
- The extended tail config is allocated to the last element in the joint list.
- In other words, if you set [param enabled] is [code]false[/code], the config of last element in the joint list has no effect in the simulated result.
+ If [param enabled] is [code]true[/code], the end bone is extended to have a tail.
+ The extended tail config is allocated to the last element in the joint list. In other words, if you set [param enabled] to [code]false[/code], the config of the last element in the joint list has no effect in the simulated result.
@@ -534,10 +533,10 @@
-
+
Sets the rotation axis at [param joint] in the bone chain's joint list when [method is_config_individual] is [code]true[/code].
- The axes are based on the [method Skeleton3D.get_bone_rest]'s space, if [param axis] is [constant ROTATION_AXIS_CUSTOM], you can specify any axis.
+ The axes are based on the [method Skeleton3D.get_bone_rest]'s space, if [param axis] is [constant SkeletonModifier3D.ROTATION_AXIS_CUSTOM], you can specify any axis.
[b]Note:[/b] The rotation axis and the forward vector shouldn't be colinear to avoid unintended rotation since [SpringBoneSimulator3D] does not factor in twisting forces.
@@ -549,7 +548,7 @@
Sets the rotation axis vector for the specified joint in the bone chain.
This vector is normalized by an internal process and represents the axis around which the bone chain can rotate.
- If the vector length is [code]0[/code], it is considered synonymous with [constant ROTATION_AXIS_ALL].
+ If the vector length is [code]0[/code], it is considered synonymous with [constant SkeletonModifier3D.ROTATION_AXIS_ALL].
@@ -597,10 +596,10 @@
-
+
Sets the rotation axis of the bone chain. If set to a specific axis, it acts like a hinge joint. The value is cached in each joint setting in the joint list.
- The axes are based on the [method Skeleton3D.get_bone_rest]'s space, if [param axis] is [constant ROTATION_AXIS_CUSTOM], you can specify any axis.
+ The axes are based on the [method Skeleton3D.get_bone_rest]'s space, if [param axis] is [constant SkeletonModifier3D.ROTATION_AXIS_CUSTOM], you can specify any axis.
[b]Note:[/b] The rotation axis vector and the forward vector shouldn't be colinear to avoid unintended rotation since [SpringBoneSimulator3D] does not factor in twisting forces.
@@ -611,7 +610,7 @@
Sets the rotation axis vector of the bone chain. The value is cached in each joint setting in the joint list.
This vector is normalized by an internal process and represents the axis around which the bone chain can rotate.
- If the vector length is [code]0[/code], it is considered synonymous with [constant ROTATION_AXIS_ALL].
+ If the vector length is [code]0[/code], it is considered synonymous with [constant SkeletonModifier3D.ROTATION_AXIS_ALL].
@@ -643,27 +642,6 @@
-
- Enumerated value for the +X axis.
-
-
- Enumerated value for the -X axis.
-
-
- Enumerated value for the +Y axis.
-
-
- Enumerated value for the -Y axis.
-
-
- Enumerated value for the +Z axis.
-
-
- Enumerated value for the -Z axis.
-
-
- Enumerated value for the axis from a parent bone to the child bone.
-
The world origin is defined as center.
@@ -675,20 +653,5 @@
The bone pose origin of the parent [Skeleton3D] specified by [method set_center_bone] is defined as center.
If [Node3D] is not found, the parent [Skeleton3D] is treated as center.
-
- Enumerated value for the rotation of the X axis.
-
-
- Enumerated value for the rotation of the Y axis.
-
-
- Enumerated value for the rotation of the Z axis.
-
-
- Enumerated value for the unconstrained rotation.
-
-
- Enumerated value for an optional rotation axis. See also [method set_joint_rotation_axis_vector].
-
diff --git a/doc/classes/TwoBoneIK3D.xml b/doc/classes/TwoBoneIK3D.xml
new file mode 100644
index 00000000000..6beed28d8f7
--- /dev/null
+++ b/doc/classes/TwoBoneIK3D.xml
@@ -0,0 +1,237 @@
+
+
+
+ Rotation based intersection of two circles inverse kinematics solver.
+
+
+ This [IKModifier3D] requires a pole target. It provides deterministic results by constructing a plane from each joint and pole target and finding the intersection of two circles (disks in 3D).
+ This IK can handle twist by setting the pole direction. If there are more than one bone between each set bone, their rotations are ignored, and the straight line connecting the root-middle and middle-end joints are treated as virtual bones.
+
+
+
+
+
+
+
+
+ Returns the end bone index.
+
+
+
+
+
+
+ Returns the end bone's tail direction when [method is_end_bone_extended] is [code]true[/code].
+
+
+
+
+
+
+ Returns the end bone tail length of the bone chain when [method is_end_bone_extended] is [code]true[/code].
+
+
+
+
+
+
+ Returns the end bone name.
+
+
+
+
+
+
+ Returns the middle bone index.
+
+
+
+
+
+
+ Returns the middle bone name.
+
+
+
+
+
+
+ Returns the pole direction.
+
+
+
+
+
+
+ Returns the pole direction vector.
+ If [method get_pole_direction] is [constant SkeletonModifier3D.SECONDARY_DIRECTION_NONE], this method returns [code]Vector3(0, 0, 0)[/code].
+
+
+
+
+
+
+ Returns the pole target node that constructs a plane which the joints are all on and the pole is trying to direct.
+
+
+
+
+
+
+ Returns the root bone index.
+
+
+
+
+
+
+ Returns the root bone name.
+
+
+
+
+
+
+ Returns the target node that the end bone is trying to reach.
+
+
+
+
+
+
+ Returns [code]true[/code] if the end bone is extended to have a tail.
+
+
+
+
+
+
+ Returns [code]true[/code] if the end bone is extended from the middle bone as a virtual bone.
+
+
+
+
+
+
+
+ Sets the end bone index.
+
+
+
+
+
+
+
+ Sets the end bone tail direction when [method is_end_bone_extended] is [code]true[/code].
+
+
+
+
+
+
+
+ Sets the end bone tail length when [method is_end_bone_extended] is [code]true[/code].
+
+
+
+
+
+
+
+ Sets the end bone name.
+ [b]Note:[/b] The end bone must be a child of the middle bone.
+
+
+
+
+
+
+
+ If [param enabled] is [code]true[/code], the end bone is extended to have a tail.
+
+
+
+
+
+
+
+ Sets the middle bone index.
+
+
+
+
+
+
+
+ Sets the middle bone name.
+ [b]Note:[/b] The middle bone must be a child of the root bone.
+
+
+
+
+
+
+
+ Sets the pole direction.
+ The pole is on the middle bone and will direct to the pole target.
+ The rotation axis is a vector that is orthogonal to this and the forward vector.
+ [b]Note:[/b] The pole direction and the forward vector shouldn't be colinear to avoid unintended rotation.
+
+
+
+
+
+
+
+ Sets the pole direction vector.
+ This vector is normalized by an internal process.
+ If the vector length is [code]0[/code], it is considered synonymous with [constant SkeletonModifier3D.SECONDARY_DIRECTION_NONE].
+
+
+
+
+
+
+
+ Sets the pole target node that constructs a plane which the joints are all on and the pole is trying to direct.
+
+
+
+
+
+
+
+ Sets the root bone index.
+
+
+
+
+
+
+
+ Sets the root bone name.
+
+
+
+
+
+
+
+ Sets the target node that the end bone is trying to reach.
+
+
+
+
+
+
+
+ If [param enabled] is [code]true[/code], the end bone is extended from the middle bone as a virtual bone.
+
+
+
+
+
+ The number of settings.
+
+
+
diff --git a/editor/icons/CCDIK3D.svg b/editor/icons/CCDIK3D.svg
new file mode 100644
index 00000000000..c2b5229ffc8
--- /dev/null
+++ b/editor/icons/CCDIK3D.svg
@@ -0,0 +1 @@
+
diff --git a/editor/icons/ChainIK3D.svg b/editor/icons/ChainIK3D.svg
new file mode 100644
index 00000000000..9247410cbe6
--- /dev/null
+++ b/editor/icons/ChainIK3D.svg
@@ -0,0 +1 @@
+
diff --git a/editor/icons/FABRIK3D.svg b/editor/icons/FABRIK3D.svg
new file mode 100644
index 00000000000..f2271f26bb9
--- /dev/null
+++ b/editor/icons/FABRIK3D.svg
@@ -0,0 +1 @@
+
diff --git a/editor/icons/IKModifier3D.svg b/editor/icons/IKModifier3D.svg
new file mode 100644
index 00000000000..512190dbdf3
--- /dev/null
+++ b/editor/icons/IKModifier3D.svg
@@ -0,0 +1 @@
+
diff --git a/editor/icons/IterateIK3D.svg b/editor/icons/IterateIK3D.svg
new file mode 100644
index 00000000000..acbb11e6d62
--- /dev/null
+++ b/editor/icons/IterateIK3D.svg
@@ -0,0 +1 @@
+
diff --git a/editor/icons/JacobianIK3D.svg b/editor/icons/JacobianIK3D.svg
new file mode 100644
index 00000000000..69be6bcb46f
--- /dev/null
+++ b/editor/icons/JacobianIK3D.svg
@@ -0,0 +1 @@
+
diff --git a/editor/icons/SplineIK3D.svg b/editor/icons/SplineIK3D.svg
new file mode 100644
index 00000000000..3e4c8ec7c49
--- /dev/null
+++ b/editor/icons/SplineIK3D.svg
@@ -0,0 +1 @@
+
diff --git a/editor/icons/TwoBoneIK3D.svg b/editor/icons/TwoBoneIK3D.svg
new file mode 100644
index 00000000000..73c6782d536
--- /dev/null
+++ b/editor/icons/TwoBoneIK3D.svg
@@ -0,0 +1 @@
+
diff --git a/editor/scene/3d/gizmos/chain_ik_3d_gizmo_plugin.cpp b/editor/scene/3d/gizmos/chain_ik_3d_gizmo_plugin.cpp
new file mode 100644
index 00000000000..3b119e1eb84
--- /dev/null
+++ b/editor/scene/3d/gizmos/chain_ik_3d_gizmo_plugin.cpp
@@ -0,0 +1,255 @@
+/**************************************************************************/
+/* chain_ik_3d_gizmo_plugin.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 "chain_ik_3d_gizmo_plugin.h"
+
+#include "editor/settings/editor_settings.h"
+
+ChainIK3DGizmoPlugin::SelectionMaterials ChainIK3DGizmoPlugin::selection_materials;
+
+ChainIK3DGizmoPlugin::ChainIK3DGizmoPlugin() {
+ selection_materials.unselected_mat.instantiate();
+ selection_materials.unselected_mat->set_shading_mode(StandardMaterial3D::SHADING_MODE_UNSHADED);
+ selection_materials.unselected_mat->set_transparency(StandardMaterial3D::TRANSPARENCY_ALPHA);
+ selection_materials.unselected_mat->set_flag(StandardMaterial3D::FLAG_ALBEDO_FROM_VERTEX_COLOR, true);
+ selection_materials.unselected_mat->set_flag(StandardMaterial3D::FLAG_SRGB_VERTEX_COLOR, true);
+ selection_materials.unselected_mat->set_flag(StandardMaterial3D::FLAG_DISABLE_FOG, true);
+
+ selection_materials.selected_mat.instantiate();
+ Ref sh;
+ sh.instantiate();
+ sh->set_code(R"(
+// Skeleton 3D gizmo bones shader.
+
+shader_type spatial;
+render_mode unshaded, shadows_disabled;
+
+void vertex() {
+ if (!OUTPUT_IS_SRGB) {
+ COLOR.rgb = mix(pow((COLOR.rgb + vec3(0.055)) * (1.0 / (1.0 + 0.055)), vec3(2.4)), COLOR.rgb * (1.0 / 12.92), lessThan(COLOR.rgb,vec3(0.04045)));
+ }
+ VERTEX = VERTEX;
+ POSITION = PROJECTION_MATRIX * VIEW_MATRIX * MODEL_MATRIX * vec4(VERTEX.xyz, 1.0);
+ POSITION.z = mix(POSITION.z, POSITION.w, 0.998);
+}
+
+void fragment() {
+ ALBEDO = COLOR.rgb;
+ ALPHA = COLOR.a;
+}
+)");
+ selection_materials.selected_mat->set_shader(sh);
+}
+
+ChainIK3DGizmoPlugin::~ChainIK3DGizmoPlugin() {
+ selection_materials.unselected_mat.unref();
+ selection_materials.selected_mat.unref();
+}
+
+bool ChainIK3DGizmoPlugin::has_gizmo(Node3D *p_spatial) {
+ return Object::cast_to(p_spatial) != nullptr;
+}
+
+String ChainIK3DGizmoPlugin::get_gizmo_name() const {
+ return "ChainIK3D";
+}
+
+int ChainIK3DGizmoPlugin::get_priority() const {
+ return -1;
+}
+
+void ChainIK3DGizmoPlugin::redraw(EditorNode3DGizmo *p_gizmo) {
+ ChainIK3D *ik = Object::cast_to(p_gizmo->get_node_3d());
+ p_gizmo->clear();
+
+ if (!ik->get_setting_count()) {
+ return;
+ }
+
+ Skeleton3D *skeleton = ik->get_skeleton();
+ if (!skeleton) {
+ return;
+ }
+
+ Ref mesh = get_joints_mesh(skeleton, ik, p_gizmo->is_selected());
+ Transform3D skel_tr = ik->get_global_transform().inverse() * skeleton->get_global_transform();
+ p_gizmo->add_mesh(mesh, Ref(), skel_tr, skeleton->register_skin(skeleton->create_skin_from_rest_transforms()));
+}
+
+Ref ChainIK3DGizmoPlugin::get_joints_mesh(Skeleton3D *p_skeleton, ChainIK3D *p_ik, bool p_is_selected) {
+ Color bone_color = EDITOR_GET("editors/3d_gizmos/gizmo_colors/ik_chain");
+ static const Color limitation_x_axis_color = Color(1, 0, 0, 1);
+ static const Color limitation_z_axis_color = Color(0, 0, 1, 1);
+
+ IterateIK3D *it_ik = Object::cast_to(p_ik);
+
+ Ref surface_tool;
+ surface_tool.instantiate();
+ surface_tool->begin(Mesh::PRIMITIVE_LINES);
+
+ if (p_is_selected) {
+ surface_tool->set_material(selection_materials.selected_mat);
+ } else {
+ selection_materials.unselected_mat->set_albedo(bone_color);
+ surface_tool->set_material(selection_materials.unselected_mat);
+ }
+
+ PackedInt32Array bones;
+ PackedFloat32Array weights;
+ bones.resize(4);
+ weights.resize(4);
+ for (int i = 0; i < 4; i++) {
+ bones.write[i] = 0;
+ weights.write[i] = 0;
+ }
+ weights.write[0] = 1;
+
+ for (int i = 0; i < p_ik->get_setting_count(); i++) {
+ int current_bone = -1;
+ int prev_bone = -1;
+ int joint_end = p_ik->get_joint_count(i) - 1;
+ bool is_extended = p_ik->is_end_bone_extended(i) && p_ik->get_end_bone_length(i) > 0;
+ for (int j = 0; j <= joint_end; j++) {
+ current_bone = p_ik->get_joint_bone(i, j);
+ Transform3D global_pose = p_skeleton->get_bone_global_rest(current_bone);
+ if (j > 0) {
+ Transform3D parent_global_pose = p_skeleton->get_bone_global_rest(prev_bone);
+ draw_line(surface_tool, parent_global_pose.origin, global_pose.origin, bone_color);
+
+ if (it_ik) {
+ // Draw rotation axis vector if not ROTATION_AXIS_ALL.
+ if (j != joint_end || (j == joint_end && is_extended)) {
+ SkeletonModifier3D::RotationAxis rotation_axis = it_ik->get_joint_rotation_axis(i, j);
+ if (rotation_axis != SkeletonModifier3D::ROTATION_AXIS_ALL) {
+ Vector3 axis_vector = it_ik->get_joint_rotation_axis_vector(i, j);
+ if (!axis_vector.is_zero_approx()) {
+ float rot_axis_length = (global_pose.origin - parent_global_pose.origin).length() * 0.2; // Use 20% of the bone length for the rotation axis vector.
+ Vector3 axis = global_pose.basis.xform(axis_vector.normalized()) * rot_axis_length;
+ draw_line(surface_tool, global_pose.origin - axis, global_pose.origin + axis, bone_color);
+ }
+ }
+ }
+
+ // Draw parent limitation shape.
+ int prev_joint = j - 1;
+ Ref lim = it_ik->get_joint_limitation(i, prev_joint);
+ if (lim.is_valid()) {
+ // Limitation space should bind parent bone rest.
+ if (prev_bone >= 0) {
+ int parent = p_skeleton->get_bone_parent(prev_bone);
+ if (parent >= 0) {
+ bones.write[0] = parent;
+ surface_tool->set_bones(bones);
+ surface_tool->set_weights(weights);
+ }
+ }
+ Transform3D tr = parent_global_pose;
+ Vector3 forward = p_skeleton->get_bone_rest(current_bone).origin;
+ tr.basis *= it_ik->get_joint_limitation_space(i, prev_joint, forward);
+ lim->draw_shape(surface_tool, tr, forward.length(), bone_color);
+ Vector3 x_axis = tr.basis.get_column(Vector3::AXIS_X).normalized() * forward.length() * 0.1;
+ Vector3 z_axis = tr.basis.get_column(Vector3::AXIS_Z).normalized() * forward.length() * 0.1;
+ draw_line(surface_tool, tr.origin + x_axis * 2, tr.origin + x_axis * 3, limitation_x_axis_color); // Offset 20%.
+ draw_line(surface_tool, tr.origin + z_axis * 2, tr.origin + z_axis * 3, limitation_z_axis_color); // Offset 20%.
+ }
+ }
+ }
+ if (j == joint_end && is_extended) {
+ Vector3 axis = p_ik->get_bone_axis(current_bone, p_ik->get_end_bone_direction(i));
+ if (axis.is_zero_approx()) {
+ continue;
+ }
+ bones.write[0] = current_bone;
+ surface_tool->set_bones(bones);
+ surface_tool->set_weights(weights);
+ float length = p_ik->get_end_bone_length(i);
+ axis = global_pose.xform(axis * length);
+ draw_line(surface_tool, global_pose.origin, axis, bone_color);
+ if (it_ik) {
+ // Draw limitation shape.
+ Ref lim = it_ik->get_joint_limitation(i, j);
+ if (lim.is_valid()) {
+ // Limitation space should bind parent bone rest.
+ if (current_bone >= 0) {
+ int parent = p_skeleton->get_bone_parent(current_bone);
+ if (parent >= 0) {
+ bones.write[0] = parent;
+ surface_tool->set_bones(bones);
+ surface_tool->set_weights(weights);
+ }
+ }
+ Vector3 forward = it_ik->get_bone_axis(current_bone, it_ik->get_end_bone_direction(i));
+ Transform3D tr = global_pose;
+ tr.basis *= it_ik->get_joint_limitation_space(i, j, forward);
+ lim->draw_shape(surface_tool, tr, length, bone_color);
+ Vector3 x_axis = tr.basis.get_column(Vector3::AXIS_X).normalized() * length * 0.1;
+ Vector3 z_axis = tr.basis.get_column(Vector3::AXIS_Z).normalized() * length * 0.1;
+ draw_line(surface_tool, tr.origin + x_axis * 2, tr.origin + x_axis * 3, limitation_x_axis_color); // Offset 20%.
+ draw_line(surface_tool, tr.origin + z_axis * 2, tr.origin + z_axis * 3, limitation_z_axis_color); // Offset 20%.
+ }
+ }
+ } else {
+ bones.write[0] = current_bone;
+ surface_tool->set_bones(bones);
+ surface_tool->set_weights(weights);
+ if (j == 0) {
+ // Check if the next bone exists.
+ int count = p_ik->get_joint_count(i);
+ if (count < 2) {
+ continue;
+ }
+ if (it_ik) {
+ // Draw rotation axis vector if not ROTATION_AXIS_ALL.
+ SkeletonModifier3D::RotationAxis rotation_axis = it_ik->get_joint_rotation_axis(i, j);
+ if (rotation_axis != SkeletonModifier3D::ROTATION_AXIS_ALL) {
+ Vector3 axis_vector = it_ik->get_joint_rotation_axis_vector(i, j);
+ if (!axis_vector.is_zero_approx()) {
+ Transform3D next_bone_global_pose = p_skeleton->get_bone_global_rest(it_ik->get_joint_bone(i, 1));
+ float rot_axis_length = (next_bone_global_pose.origin - global_pose.origin).length() * 0.2; // Use 20% of the bone length for the rotation axis vector.
+ Vector3 axis = global_pose.basis.xform(axis_vector.normalized()) * rot_axis_length;
+ draw_line(surface_tool, global_pose.origin - axis, global_pose.origin + axis, bone_color);
+ }
+ }
+ }
+ }
+ }
+ prev_bone = current_bone;
+ }
+ }
+
+ return surface_tool->commit();
+}
+
+void ChainIK3DGizmoPlugin::draw_line(Ref &p_surface_tool, const Vector3 &p_begin_pos, const Vector3 &p_end_pos, const Color &p_color) {
+ p_surface_tool->set_color(p_color);
+ p_surface_tool->add_vertex(p_begin_pos);
+ p_surface_tool->set_color(p_color);
+ p_surface_tool->add_vertex(p_end_pos);
+}
diff --git a/editor/scene/3d/gizmos/chain_ik_3d_gizmo_plugin.h b/editor/scene/3d/gizmos/chain_ik_3d_gizmo_plugin.h
new file mode 100644
index 00000000000..611374aa877
--- /dev/null
+++ b/editor/scene/3d/gizmos/chain_ik_3d_gizmo_plugin.h
@@ -0,0 +1,60 @@
+/**************************************************************************/
+/* chain_ik_3d_gizmo_plugin.h */
+/**************************************************************************/
+/* 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. */
+/**************************************************************************/
+
+#pragma once
+
+#include "editor/plugins/editor_plugin.h"
+#include "editor/scene/3d/node_3d_editor_plugin.h"
+#include "scene/3d/iterate_ik_3d.h"
+
+#include "scene/resources/surface_tool.h"
+
+class ChainIK3DGizmoPlugin : public EditorNode3DGizmoPlugin {
+ GDCLASS(ChainIK3DGizmoPlugin, EditorNode3DGizmoPlugin);
+
+ struct SelectionMaterials {
+ Ref unselected_mat;
+ Ref selected_mat;
+ };
+ static SelectionMaterials selection_materials;
+
+public:
+ static Ref get_joints_mesh(Skeleton3D *p_skeleton, ChainIK3D *p_ik, bool p_is_selected);
+ static void draw_line(Ref &p_surface_tool, const Vector3 &p_begin_pos, const Vector3 &p_end_pos, const Color &p_color);
+
+ bool has_gizmo(Node3D *p_spatial) override;
+ String get_gizmo_name() const override;
+ int get_priority() const override;
+
+ void redraw(EditorNode3DGizmo *p_gizmo) override;
+
+ ChainIK3DGizmoPlugin();
+ ~ChainIK3DGizmoPlugin();
+};
diff --git a/editor/scene/3d/gizmos/spring_bone_3d_gizmo_plugin.cpp b/editor/scene/3d/gizmos/spring_bone_3d_gizmo_plugin.cpp
index f9e00d6875b..f914aaff11e 100644
--- a/editor/scene/3d/gizmos/spring_bone_3d_gizmo_plugin.cpp
+++ b/editor/scene/3d/gizmos/spring_bone_3d_gizmo_plugin.cpp
@@ -55,14 +55,16 @@ SpringBoneSimulator3DGizmoPlugin::SpringBoneSimulator3DGizmoPlugin() {
shader_type spatial;
render_mode unshaded, shadows_disabled;
+
void vertex() {
if (!OUTPUT_IS_SRGB) {
- COLOR.rgb = mix( pow((COLOR.rgb + vec3(0.055)) * (1.0 / (1.0 + 0.055)), vec3(2.4)), COLOR.rgb* (1.0 / 12.92), lessThan(COLOR.rgb,vec3(0.04045)) );
+ COLOR.rgb = mix(pow((COLOR.rgb + vec3(0.055)) * (1.0 / (1.0 + 0.055)), vec3(2.4)), COLOR.rgb * (1.0 / 12.92), lessThan(COLOR.rgb,vec3(0.04045)));
}
VERTEX = VERTEX;
POSITION = PROJECTION_MATRIX * VIEW_MATRIX * MODEL_MATRIX * vec4(VERTEX.xyz, 1.0);
POSITION.z = mix(POSITION.z, POSITION.w, 0.998);
}
+
void fragment() {
ALBEDO = COLOR.rgb;
ALPHA = COLOR.a;
@@ -145,8 +147,8 @@ Ref SpringBoneSimulator3DGizmoPlugin::get_joints_mesh(Skeleton3D *p_s
// Draw rotation axis vector if not ROTATION_AXIS_ALL.
if (j != joint_end || (j == joint_end && is_extended)) {
- SpringBoneSimulator3D::RotationAxis rotation_axis = p_simulator->get_joint_rotation_axis(i, j);
- if (rotation_axis != SpringBoneSimulator3D::ROTATION_AXIS_ALL) {
+ SkeletonModifier3D::RotationAxis rotation_axis = p_simulator->get_joint_rotation_axis(i, j);
+ if (rotation_axis != SkeletonModifier3D::ROTATION_AXIS_ALL) {
Vector3 axis_vector = p_simulator->get_joint_rotation_axis_vector(i, j);
if (!axis_vector.is_zero_approx()) {
float line_length = p_simulator->get_joint_radius(i, j - 1) * 2.0;
@@ -173,8 +175,8 @@ Ref SpringBoneSimulator3DGizmoPlugin::get_joints_mesh(Skeleton3D *p_s
surface_tool->set_weights(Vector(weights));
if (j == 0) {
// Draw rotation axis vector if not ROTATION_AXIS_ALL.
- SpringBoneSimulator3D::RotationAxis rotation_axis = p_simulator->get_joint_rotation_axis(i, j);
- if (rotation_axis != SpringBoneSimulator3D::ROTATION_AXIS_ALL) {
+ SkeletonModifier3D::RotationAxis rotation_axis = p_simulator->get_joint_rotation_axis(i, j);
+ if (rotation_axis != SkeletonModifier3D::ROTATION_AXIS_ALL) {
Vector3 axis_vector = p_simulator->get_joint_rotation_axis_vector(i, j);
if (!axis_vector.is_zero_approx()) {
float line_length = p_simulator->get_joint_radius(i, j) * 2.0;
@@ -242,14 +244,16 @@ SpringBoneCollision3DGizmoPlugin::SpringBoneCollision3DGizmoPlugin() {
shader_type spatial;
render_mode unshaded, shadows_disabled;
+
void vertex() {
if (!OUTPUT_IS_SRGB) {
- COLOR.rgb = mix( pow((COLOR.rgb + vec3(0.055)) * (1.0 / (1.0 + 0.055)), vec3(2.4)), COLOR.rgb* (1.0 / 12.92), lessThan(COLOR.rgb,vec3(0.04045)) );
+ COLOR.rgb = mix(pow((COLOR.rgb + vec3(0.055)) * (1.0 / (1.0 + 0.055)), vec3(2.4)), COLOR.rgb * (1.0 / 12.92), lessThan(COLOR.rgb,vec3(0.04045)));
}
VERTEX = VERTEX;
POSITION = PROJECTION_MATRIX * VIEW_MATRIX * MODEL_MATRIX * vec4(VERTEX.xyz, 1.0);
POSITION.z = mix(POSITION.z, POSITION.w, 0.998);
}
+
void fragment() {
ALBEDO = COLOR.rgb;
ALPHA = COLOR.a;
diff --git a/editor/scene/3d/gizmos/two_bone_ik_3d_gizmo_plugin.cpp b/editor/scene/3d/gizmos/two_bone_ik_3d_gizmo_plugin.cpp
new file mode 100644
index 00000000000..101b96f4909
--- /dev/null
+++ b/editor/scene/3d/gizmos/two_bone_ik_3d_gizmo_plugin.cpp
@@ -0,0 +1,209 @@
+/**************************************************************************/
+/* two_bone_ik_3d_gizmo_plugin.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 "two_bone_ik_3d_gizmo_plugin.h"
+
+#include "editor/settings/editor_settings.h"
+
+TwoBoneIK3DGizmoPlugin::SelectionMaterials TwoBoneIK3DGizmoPlugin::selection_materials;
+
+TwoBoneIK3DGizmoPlugin::TwoBoneIK3DGizmoPlugin() {
+ selection_materials.unselected_mat.instantiate();
+ selection_materials.unselected_mat->set_shading_mode(StandardMaterial3D::SHADING_MODE_UNSHADED);
+ selection_materials.unselected_mat->set_transparency(StandardMaterial3D::TRANSPARENCY_ALPHA);
+ selection_materials.unselected_mat->set_flag(StandardMaterial3D::FLAG_ALBEDO_FROM_VERTEX_COLOR, true);
+ selection_materials.unselected_mat->set_flag(StandardMaterial3D::FLAG_SRGB_VERTEX_COLOR, true);
+ selection_materials.unselected_mat->set_flag(StandardMaterial3D::FLAG_DISABLE_FOG, true);
+
+ selection_materials.selected_mat.instantiate();
+ Ref sh;
+ sh.instantiate();
+ sh->set_code(R"(
+// Skeleton 3D gizmo bones shader.
+
+shader_type spatial;
+render_mode unshaded, shadows_disabled;
+
+void vertex() {
+ if (!OUTPUT_IS_SRGB) {
+ COLOR.rgb = mix(pow((COLOR.rgb + vec3(0.055)) * (1.0 / (1.0 + 0.055)), vec3(2.4)), COLOR.rgb * (1.0 / 12.92), lessThan(COLOR.rgb,vec3(0.04045)));
+ }
+ VERTEX = VERTEX;
+ POSITION = PROJECTION_MATRIX * VIEW_MATRIX * MODEL_MATRIX * vec4(VERTEX.xyz, 1.0);
+ POSITION.z = mix(POSITION.z, POSITION.w, 0.998);
+}
+
+void fragment() {
+ ALBEDO = COLOR.rgb;
+ ALPHA = COLOR.a;
+}
+)");
+ selection_materials.selected_mat->set_shader(sh);
+}
+
+TwoBoneIK3DGizmoPlugin::~TwoBoneIK3DGizmoPlugin() {
+ selection_materials.unselected_mat.unref();
+ selection_materials.selected_mat.unref();
+}
+
+bool TwoBoneIK3DGizmoPlugin::has_gizmo(Node3D *p_spatial) {
+ return Object::cast_to(p_spatial) != nullptr;
+}
+
+String TwoBoneIK3DGizmoPlugin::get_gizmo_name() const {
+ return "TwoBoneIK3D";
+}
+
+int TwoBoneIK3DGizmoPlugin::get_priority() const {
+ return -1;
+}
+
+void TwoBoneIK3DGizmoPlugin::redraw(EditorNode3DGizmo *p_gizmo) {
+ TwoBoneIK3D *ik = Object::cast_to(p_gizmo->get_node_3d());
+ p_gizmo->clear();
+
+ if (!ik->get_setting_count()) {
+ return;
+ }
+
+ Skeleton3D *skeleton = ik->get_skeleton();
+ if (!skeleton) {
+ return;
+ }
+
+ Ref mesh = get_joints_mesh(skeleton, ik, p_gizmo->is_selected());
+ Transform3D skel_tr = ik->get_global_transform().inverse() * skeleton->get_global_transform();
+ p_gizmo->add_mesh(mesh, Ref(), skel_tr, skeleton->register_skin(skeleton->create_skin_from_rest_transforms()));
+}
+
+Ref TwoBoneIK3DGizmoPlugin::get_joints_mesh(Skeleton3D *p_skeleton, TwoBoneIK3D *p_ik, bool p_is_selected) {
+ Color bone_color = EDITOR_GET("editors/3d_gizmos/gizmo_colors/ik_chain");
+
+ Ref surface_tool;
+ surface_tool.instantiate();
+ surface_tool->begin(Mesh::PRIMITIVE_LINES);
+
+ if (p_is_selected) {
+ surface_tool->set_material(selection_materials.selected_mat);
+ } else {
+ selection_materials.unselected_mat->set_albedo(bone_color);
+ surface_tool->set_material(selection_materials.unselected_mat);
+ }
+
+ PackedInt32Array bones;
+ PackedFloat32Array weights;
+ bones.resize(4);
+ weights.resize(4);
+ for (int i = 0; i < 4; i++) {
+ bones.write[i] = 0;
+ weights.write[i] = 0;
+ }
+ weights.write[0] = 1;
+
+ for (int i = 0; i < p_ik->get_setting_count(); i++) {
+ if (!p_ik->is_valid(i)) {
+ continue; // Skip invalid settings.
+ }
+
+ int root_bone = p_ik->get_root_bone(i);
+ int middle_bone = p_ik->get_middle_bone(i);
+ int end_bone = p_ik->get_end_bone(i);
+
+ bool is_extended = p_ik->is_end_bone_extended(i) && p_ik->get_end_bone_length(i) > 0.0f;
+
+ Transform3D root_gp = p_skeleton->get_bone_global_rest(root_bone);
+ Transform3D mid_gp = p_skeleton->get_bone_global_rest(middle_bone);
+ Transform3D end_gp = p_skeleton->get_bone_global_rest(end_bone);
+
+ Vector3 end_point = end_gp.origin;
+ if (is_extended) {
+ end_point += end_gp.basis.get_rotation_quaternion().xform(p_ik->get_bone_axis(end_bone, p_ik->get_end_bone_direction(i))).normalized() * p_ik->get_end_bone_length(i);
+ }
+
+ bones.write[0] = root_bone;
+ surface_tool->set_bones(bones);
+ surface_tool->set_weights(weights);
+ draw_line(surface_tool, root_gp.origin, mid_gp.origin, bone_color);
+
+ bones.write[0] = middle_bone;
+ surface_tool->set_bones(bones);
+ surface_tool->set_weights(weights);
+ draw_line(surface_tool, mid_gp.origin, end_point, bone_color);
+
+ Vector3 pole_vector = p_ik->get_pole_direction_vector(i);
+ if (pole_vector.is_zero_approx()) {
+ continue;
+ }
+
+ float pole_length = MIN(root_gp.origin.distance_to(mid_gp.origin), mid_gp.origin.distance_to(end_point)) * 0.25;
+ draw_arrow(surface_tool, mid_gp.origin, mid_gp.basis.get_rotation_quaternion().xform(pole_vector).normalized(), pole_length, bone_color);
+ }
+
+ return surface_tool->commit();
+}
+
+void TwoBoneIK3DGizmoPlugin::draw_line(Ref &p_surface_tool, const Vector3 &p_begin_pos, const Vector3 &p_end_pos, const Color &p_color) {
+ p_surface_tool->set_color(p_color);
+ p_surface_tool->add_vertex(p_begin_pos);
+ p_surface_tool->set_color(p_color);
+ p_surface_tool->add_vertex(p_end_pos);
+}
+
+void TwoBoneIK3DGizmoPlugin::draw_arrow(Ref &p_surface_tool, const Vector3 &p_origin, const Vector3 &p_direction, real_t p_length, const Color &p_color) {
+ static const float HALF_PI = Math::PI * 0.5;
+
+ p_surface_tool->set_color(p_color);
+
+ LocalVector arrow_body;
+ arrow_body.resize(2);
+ arrow_body[0] = Vector3(0, 0, 0);
+ arrow_body[1] = Vector3(0, p_length, 0);
+
+ LocalVector arrow_head;
+ arrow_head.resize(2);
+ arrow_head[0] = Vector3(0, p_length, 0);
+ arrow_head[1] = Vector3(p_length * 0.25, p_length * 0.75, 0);
+
+ Quaternion dir = Quaternion(Vector3(0, 1, 0), p_direction);
+
+ // Draw the arrow body.
+ p_surface_tool->add_vertex(arrow_body[0] + p_origin);
+ p_surface_tool->add_vertex(dir.xform(arrow_body[1]) + p_origin);
+
+ // Draw the arrow head 4 times, rotate around the arrow body.
+ for (int i = 0; i < 4; i++) {
+ Quaternion rotation = dir * Quaternion(Vector3(0, 1, 0), HALF_PI * i);
+ for (int j = 0; j < 2; j++) {
+ Vector3 v = arrow_head[j];
+ Vector3 rotated_v = rotation.xform(v);
+ p_surface_tool->add_vertex(rotated_v + p_origin);
+ }
+ }
+}
diff --git a/editor/scene/3d/gizmos/two_bone_ik_3d_gizmo_plugin.h b/editor/scene/3d/gizmos/two_bone_ik_3d_gizmo_plugin.h
new file mode 100644
index 00000000000..bec19b39dc8
--- /dev/null
+++ b/editor/scene/3d/gizmos/two_bone_ik_3d_gizmo_plugin.h
@@ -0,0 +1,61 @@
+/**************************************************************************/
+/* two_bone_ik_3d_gizmo_plugin.h */
+/**************************************************************************/
+/* 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. */
+/**************************************************************************/
+
+#pragma once
+
+#include "editor/plugins/editor_plugin.h"
+#include "editor/scene/3d/node_3d_editor_plugin.h"
+#include "scene/3d/two_bone_ik_3d.h"
+
+#include "scene/resources/surface_tool.h"
+
+class TwoBoneIK3DGizmoPlugin : public EditorNode3DGizmoPlugin {
+ GDCLASS(TwoBoneIK3DGizmoPlugin, EditorNode3DGizmoPlugin);
+
+ struct SelectionMaterials {
+ Ref unselected_mat;
+ Ref selected_mat;
+ };
+ static SelectionMaterials selection_materials;
+
+public:
+ static Ref get_joints_mesh(Skeleton3D *p_skeleton, TwoBoneIK3D *p_ik, bool p_is_selected);
+ static void draw_line(Ref &p_surface_tool, const Vector3 &p_begin_pos, const Vector3 &p_end_pos, const Color &p_color);
+ static void draw_arrow(Ref &p_surface_tool, const Vector3 &p_origin, const Vector3 &p_direction, real_t p_length, const Color &p_color);
+
+ bool has_gizmo(Node3D *p_spatial) override;
+ String get_gizmo_name() const override;
+ int get_priority() const override;
+
+ void redraw(EditorNode3DGizmo *p_gizmo) override;
+
+ TwoBoneIK3DGizmoPlugin();
+ ~TwoBoneIK3DGizmoPlugin();
+};
diff --git a/editor/scene/3d/node_3d_editor_plugin.cpp b/editor/scene/3d/node_3d_editor_plugin.cpp
index 079150bca49..f89b31debcb 100644
--- a/editor/scene/3d/node_3d_editor_plugin.cpp
+++ b/editor/scene/3d/node_3d_editor_plugin.cpp
@@ -48,6 +48,7 @@
#include "editor/scene/3d/gizmos/audio_listener_3d_gizmo_plugin.h"
#include "editor/scene/3d/gizmos/audio_stream_player_3d_gizmo_plugin.h"
#include "editor/scene/3d/gizmos/camera_3d_gizmo_plugin.h"
+#include "editor/scene/3d/gizmos/chain_ik_3d_gizmo_plugin.h"
#include "editor/scene/3d/gizmos/cpu_particles_3d_gizmo_plugin.h"
#include "editor/scene/3d/gizmos/decal_gizmo_plugin.h"
#include "editor/scene/3d/gizmos/fog_volume_gizmo_plugin.h"
@@ -75,6 +76,7 @@
#include "editor/scene/3d/gizmos/reflection_probe_gizmo_plugin.h"
#include "editor/scene/3d/gizmos/spring_bone_3d_gizmo_plugin.h"
#include "editor/scene/3d/gizmos/sprite_base_3d_gizmo_plugin.h"
+#include "editor/scene/3d/gizmos/two_bone_ik_3d_gizmo_plugin.h"
#include "editor/scene/3d/gizmos/visible_on_screen_notifier_3d_gizmo_plugin.h"
#include "editor/scene/3d/gizmos/voxel_gi_gizmo_plugin.h"
#include "editor/scene/3d/node_3d_editor_gizmos.h"
@@ -9234,6 +9236,8 @@ void Node3DEditor::_register_all_gizmos() {
add_gizmo_plugin(Ref(memnew(Joint3DGizmoPlugin)));
add_gizmo_plugin(Ref(memnew(PhysicalBone3DGizmoPlugin)));
add_gizmo_plugin(Ref(memnew(FogVolumeGizmoPlugin)));
+ add_gizmo_plugin(Ref(memnew(TwoBoneIK3DGizmoPlugin)));
+ add_gizmo_plugin(Ref(memnew(ChainIK3DGizmoPlugin)));
}
void Node3DEditor::_bind_methods() {
diff --git a/editor/scene/3d/skeleton_3d_editor_plugin.cpp b/editor/scene/3d/skeleton_3d_editor_plugin.cpp
index 366b1ff32fd..ed36aa63956 100644
--- a/editor/scene/3d/skeleton_3d_editor_plugin.cpp
+++ b/editor/scene/3d/skeleton_3d_editor_plugin.cpp
@@ -1474,14 +1474,16 @@ Skeleton3DGizmoPlugin::Skeleton3DGizmoPlugin() {
shader_type spatial;
render_mode unshaded, shadows_disabled;
+
void vertex() {
if (!OUTPUT_IS_SRGB) {
- COLOR.rgb = mix( pow((COLOR.rgb + vec3(0.055)) * (1.0 / (1.0 + 0.055)), vec3(2.4)), COLOR.rgb* (1.0 / 12.92), lessThan(COLOR.rgb,vec3(0.04045)) );
+ COLOR.rgb = mix(pow((COLOR.rgb + vec3(0.055)) * (1.0 / (1.0 + 0.055)), vec3(2.4)), COLOR.rgb * (1.0 / 12.92), lessThan(COLOR.rgb,vec3(0.04045)));
}
VERTEX = VERTEX;
POSITION = PROJECTION_MATRIX * VIEW_MATRIX * MODEL_MATRIX * vec4(VERTEX.xyz, 1.0);
POSITION.z = mix(POSITION.z, POSITION.w, 0.998);
}
+
void fragment() {
ALBEDO = COLOR.rgb;
ALPHA = COLOR.a;
diff --git a/editor/settings/editor_build_profile.cpp b/editor/settings/editor_build_profile.cpp
index 625328a8e4f..0f5f4abd95b 100644
--- a/editor/settings/editor_build_profile.cpp
+++ b/editor/settings/editor_build_profile.cpp
@@ -301,8 +301,6 @@ const HashMap> EditorBuildP
"RayCast3D",
"SoftBody3D",
"SpringArm3D",
- "SpringBoneCollision3D",
- "SpringBoneSimulator3D",
"VehicleWheel3D",
} },
{ BUILD_OPTION_TEXT_SERVER_ADVANCED, {
diff --git a/editor/settings/editor_settings.cpp b/editor/settings/editor_settings.cpp
index d666a02057c..cf4182a889f 100644
--- a/editor/settings/editor_settings.cpp
+++ b/editor/settings/editor_settings.cpp
@@ -895,6 +895,7 @@ void EditorSettings::_load_defaults(Ref p_extra_config) {
EDITOR_SETTING_USAGE(Variant::COLOR, PROPERTY_HINT_NONE, "editors/3d_gizmos/gizmo_colors/spring_bone_joint", Color(0.8, 0.9, 0.6), "", PROPERTY_USAGE_DEFAULT | PROPERTY_USAGE_RESTART_IF_CHANGED)
EDITOR_SETTING_USAGE(Variant::COLOR, PROPERTY_HINT_NONE, "editors/3d_gizmos/gizmo_colors/spring_bone_collision", Color(0.6, 0.8, 0.9), "", PROPERTY_USAGE_DEFAULT | PROPERTY_USAGE_RESTART_IF_CHANGED)
EDITOR_SETTING_USAGE(Variant::COLOR, PROPERTY_HINT_NONE, "editors/3d_gizmos/gizmo_colors/spring_bone_inside_collision", Color(0.9, 0.6, 0.8), "", PROPERTY_USAGE_DEFAULT | PROPERTY_USAGE_RESTART_IF_CHANGED)
+ EDITOR_SETTING_USAGE(Variant::COLOR, PROPERTY_HINT_NONE, "editors/3d_gizmos/gizmo_colors/ik_chain", Color(0.6, 0.9, 0.8), "", PROPERTY_USAGE_DEFAULT | PROPERTY_USAGE_RESTART_IF_CHANGED)
_initial_set("editors/3d_gizmos/gizmo_settings/bone_axis_length", (float)0.1);
EDITOR_SETTING(Variant::INT, PROPERTY_HINT_ENUM, "editors/3d_gizmos/gizmo_settings/bone_shape", 1, "Wire,Octahedron");
EDITOR_SETTING_USAGE(Variant::FLOAT, PROPERTY_HINT_NONE, "editors/3d_gizmos/gizmo_settings/path3d_tilt_disk_size", 0.8, "", PROPERTY_USAGE_DEFAULT | PROPERTY_USAGE_RESTART_IF_CHANGED)
diff --git a/misc/extension_api_validation/4.5-stable.expected b/misc/extension_api_validation/4.5-stable.expected
index 4fef411d057..2f95776d61f 100644
--- a/misc/extension_api_validation/4.5-stable.expected
+++ b/misc/extension_api_validation/4.5-stable.expected
@@ -68,3 +68,17 @@ GH-110433
Validate extension JSON: Error: Field 'classes/Performance/methods/add_custom_monitor/arguments': size changed value in new API, from 3 to 4.
Optional argument added. Compatibility method registered.
+
+
+GH-110120
+---------
+Validate extension JSON: API was removed: classes/SpringBoneSimulator3D/enums/BoneDirection
+Validate extension JSON: API was removed: classes/SpringBoneSimulator3D/enums/RotationAxis
+Validate extension JSON: Error: Field 'classes/SpringBoneSimulator3D/methods/get_end_bone_direction/return_value': type changed value in new API, from "enum::SpringBoneSimulator3D.BoneDirection" to "enum::SkeletonModifier3D.BoneDirection".
+Validate extension JSON: Error: Field 'classes/SpringBoneSimulator3D/methods/get_joint_rotation_axis/return_value': type changed value in new API, from "enum::SpringBoneSimulator3D.RotationAxis" to "enum::SkeletonModifier3D.RotationAxis".
+Validate extension JSON: Error: Field 'classes/SpringBoneSimulator3D/methods/get_rotation_axis/return_value': type changed value in new API, from "enum::SpringBoneSimulator3D.RotationAxis" to "enum::SkeletonModifier3D.RotationAxis".
+Validate extension JSON: Error: Field 'classes/SpringBoneSimulator3D/methods/set_end_bone_direction/arguments/1': type changed value in new API, from "enum::SpringBoneSimulator3D.BoneDirection" to "enum::SkeletonModifier3D.BoneDirection".
+Validate extension JSON: Error: Field 'classes/SpringBoneSimulator3D/methods/set_joint_rotation_axis/arguments/2': type changed value in new API, from "enum::SpringBoneSimulator3D.RotationAxis" to "enum::SkeletonModifier3D.RotationAxis".
+Validate extension JSON: Error: Field 'classes/SpringBoneSimulator3D/methods/set_rotation_axis/arguments/1': type changed value in new API, from "enum::SpringBoneSimulator3D.RotationAxis" to "enum::SkeletonModifier3D.RotationAxis".
+
+Same enum is defined in SkeletonModifier3D which is a base class of SpringBoneSimulator3D. Compatibility methods registered.
diff --git a/scene/3d/aim_modifier_3d.cpp b/scene/3d/aim_modifier_3d.cpp
index c6f4730672e..71e3a872da8 100644
--- a/scene/3d/aim_modifier_3d.cpp
+++ b/scene/3d/aim_modifier_3d.cpp
@@ -84,7 +84,7 @@ void AimModifier3D::_get_property_list(List *p_list) const {
String path = "settings/" + itos(i) + "/";
int rotation_usage = is_using_euler(i) ? PROPERTY_USAGE_DEFAULT : PROPERTY_USAGE_NONE;
- p_list->push_back(PropertyInfo(Variant::INT, path + "forward_axis", PROPERTY_HINT_ENUM, "+X,-X,+Y,-Y,+Z,-Z"));
+ p_list->push_back(PropertyInfo(Variant::INT, path + "forward_axis", PROPERTY_HINT_ENUM, SkeletonModifier3D::get_hint_bone_axis()));
p_list->push_back(PropertyInfo(Variant::BOOL, path + "use_euler"));
p_list->push_back(PropertyInfo(Variant::INT, path + "primary_rotation_axis", PROPERTY_HINT_ENUM, "X,Y,Z", rotation_usage));
p_list->push_back(PropertyInfo(Variant::BOOL, path + "use_secondary_rotation", PROPERTY_HINT_NONE, "", rotation_usage));
diff --git a/scene/3d/bone_constraint_3d.cpp b/scene/3d/bone_constraint_3d.cpp
index 19c80633716..5bad937633e 100644
--- a/scene/3d/bone_constraint_3d.cpp
+++ b/scene/3d/bone_constraint_3d.cpp
@@ -270,45 +270,6 @@ void BoneConstraint3D::_process_constraint(int p_index, Skeleton3D *p_skeleton,
//
}
-double BoneConstraint3D::symmetrize_angle(double p_angle) {
- double angle = Math::fposmod(p_angle, Math::TAU);
- return angle > Math::PI ? angle - Math::TAU : angle;
-}
-
-double BoneConstraint3D::get_roll_angle(const Quaternion &p_rotation, const Vector3 &p_roll_axis) {
- // Ensure roll axis is normalized.
- Vector3 roll_axis = p_roll_axis.normalized();
-
- // Project the quaternion rotation onto the roll axis.
- // This gives us the component of rotation around that axis.
- double dot = p_rotation.x * roll_axis.x +
- p_rotation.y * roll_axis.y +
- p_rotation.z * roll_axis.z;
-
- // Create a quaternion representing just the roll component.
- Quaternion roll_component;
- roll_component.x = roll_axis.x * dot;
- roll_component.y = roll_axis.y * dot;
- roll_component.z = roll_axis.z * dot;
- roll_component.w = p_rotation.w;
-
- // Normalize this component.
- double length = roll_component.length();
- if (length > CMP_EPSILON) {
- roll_component = roll_component / length;
- } else {
- return 0.0;
- }
-
- // Extract the angle.
- double angle = 2.0 * Math::acos(CLAMP(roll_component.w, -1.0, 1.0));
-
- // Determine the sign.
- double direction = (roll_component.x * roll_axis.x + roll_component.y * roll_axis.y + roll_component.z * roll_axis.z > 0) ? 1.0 : -1.0;
-
- return symmetrize_angle(angle * direction);
-}
-
BoneConstraint3D::~BoneConstraint3D() {
clear_settings();
}
diff --git a/scene/3d/bone_constraint_3d.h b/scene/3d/bone_constraint_3d.h
index 1129c881b1d..3da28f24ab7 100644
--- a/scene/3d/bone_constraint_3d.h
+++ b/scene/3d/bone_constraint_3d.h
@@ -83,8 +83,5 @@ public:
void clear_settings();
- static double symmetrize_angle(double p_angle); // Helper to make angle 0->TAU become -PI->PI.
- static double get_roll_angle(const Quaternion &p_rotation, const Vector3 &p_roll_axis);
-
~BoneConstraint3D();
};
diff --git a/scene/3d/ccd_ik_3d.cpp b/scene/3d/ccd_ik_3d.cpp
new file mode 100644
index 00000000000..71b7a19c8dd
--- /dev/null
+++ b/scene/3d/ccd_ik_3d.cpp
@@ -0,0 +1,71 @@
+/**************************************************************************/
+/* ccd_ik_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 "ccd_ik_3d.h"
+
+void CCDIK3D::_solve_iteration(double p_delta, Skeleton3D *p_skeleton, IterateIK3DSetting *p_setting, const Vector3 &p_destination) {
+ int joint_size = (int)p_setting->joints.size();
+ int chain_size = (int)p_setting->chain.size();
+
+ // Backwards.
+ for (int ancestor = joint_size - 1; ancestor >= 0; ancestor--) {
+ // Forwards.
+ for (int i = ancestor; i < joint_size; i++) {
+ IKModifier3DSolverInfo *solver_info = p_setting->solver_info_list[i];
+ if (!solver_info || Math::is_zero_approx(solver_info->length)) {
+ continue;
+ }
+
+ int HEAD = i;
+ int TAIL = i + 1;
+
+ Vector3 current_head = p_setting->chain[HEAD];
+ Vector3 current_effector = p_setting->chain[chain_size - 1];
+ Vector3 head_to_effector = current_effector - current_head;
+ Vector3 head_to_destination = p_destination - current_head;
+
+ if (Math::is_zero_approx(head_to_destination.length_squared() * head_to_effector.length_squared())) {
+ continue;
+ }
+
+ Quaternion to_rot = Quaternion(head_to_effector.normalized(), head_to_destination.normalized());
+ Vector3 to_tail = p_setting->chain[TAIL] - current_head;
+
+ p_setting->update_chain_coordinate_fw(p_skeleton, TAIL, current_head + to_rot.xform(to_tail));
+
+ if (p_setting->joint_settings[HEAD]->rotation_axis != ROTATION_AXIS_ALL) {
+ p_setting->update_chain_coordinate_fw(p_skeleton, TAIL, p_setting->chain[HEAD] + p_setting->joint_settings[HEAD]->get_projected_rotation(solver_info->current_grest, p_setting->chain[TAIL] - p_setting->chain[HEAD]));
+ }
+ if (p_setting->joint_settings[HEAD]->limitation.is_valid()) {
+ p_setting->update_chain_coordinate_fw(p_skeleton, TAIL, p_setting->chain[HEAD] + p_setting->joint_settings[HEAD]->get_limited_rotation(solver_info->current_grest, p_setting->chain[TAIL] - p_setting->chain[HEAD], solver_info->forward_vector));
+ }
+ }
+ }
+}
diff --git a/scene/3d/ccd_ik_3d.h b/scene/3d/ccd_ik_3d.h
new file mode 100644
index 00000000000..aea340043db
--- /dev/null
+++ b/scene/3d/ccd_ik_3d.h
@@ -0,0 +1,40 @@
+/**************************************************************************/
+/* ccd_ik_3d.h */
+/**************************************************************************/
+/* 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. */
+/**************************************************************************/
+
+#pragma once
+
+#include "scene/3d/iterate_ik_3d.h"
+
+class CCDIK3D : public IterateIK3D {
+ GDCLASS(CCDIK3D, IterateIK3D);
+
+protected:
+ virtual void _solve_iteration(double p_delta, Skeleton3D *p_skeleton, IterateIK3DSetting *p_setting, const Vector3 &p_destination) override;
+};
diff --git a/scene/3d/chain_ik_3d.cpp b/scene/3d/chain_ik_3d.cpp
new file mode 100644
index 00000000000..eb132bec99d
--- /dev/null
+++ b/scene/3d/chain_ik_3d.cpp
@@ -0,0 +1,485 @@
+/**************************************************************************/
+/* chain_ik_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 "chain_ik_3d.h"
+
+bool ChainIK3D::_set(const StringName &p_path, const Variant &p_value) {
+ String path = p_path;
+
+ if (path.begins_with("settings/")) {
+ int which = path.get_slicec('/', 1).to_int();
+ String what = path.get_slicec('/', 2);
+ ERR_FAIL_INDEX_V(which, (int)settings.size(), false);
+
+ if (what == "root_bone_name") {
+ set_root_bone_name(which, p_value);
+ } else if (what == "root_bone") {
+ set_root_bone(which, p_value);
+ } else if (what == "end_bone_name") {
+ set_end_bone_name(which, p_value);
+ } else if (what == "end_bone") {
+ String opt = path.get_slicec('/', 3);
+ if (opt.is_empty()) {
+ set_end_bone(which, p_value);
+ } else if (opt == "direction") {
+ set_end_bone_direction(which, static_cast((int)p_value));
+ } else if (opt == "length") {
+ set_end_bone_length(which, p_value);
+ } else {
+ return false;
+ }
+ } else if (what == "extend_end_bone") {
+ set_extend_end_bone(which, p_value);
+ } else if (what == "joint_count") {
+ set_joint_count(which, p_value);
+ } else if (what == "joints") {
+ int idx = path.get_slicec('/', 3).to_int();
+ String prop = path.get_slicec('/', 4);
+ if (prop == "bone_name") {
+ set_joint_bone_name(which, idx, p_value);
+ } else if (prop == "bone") {
+ set_joint_bone(which, idx, p_value);
+ } else {
+ return false;
+ }
+ } else {
+ return false;
+ }
+ }
+ return true;
+}
+
+bool ChainIK3D::_get(const StringName &p_path, Variant &r_ret) const {
+ String path = p_path;
+
+ if (path.begins_with("settings/")) {
+ int which = path.get_slicec('/', 1).to_int();
+ String what = path.get_slicec('/', 2);
+ ERR_FAIL_INDEX_V(which, (int)settings.size(), false);
+
+ if (what == "root_bone_name") {
+ r_ret = get_root_bone_name(which);
+ } else if (what == "root_bone") {
+ r_ret = get_root_bone(which);
+ } else if (what == "end_bone_name") {
+ r_ret = get_end_bone_name(which);
+ } else if (what == "end_bone") {
+ String opt = path.get_slicec('/', 3);
+ if (opt.is_empty()) {
+ r_ret = get_end_bone(which);
+ } else if (opt == "direction") {
+ r_ret = (int)get_end_bone_direction(which);
+ } else if (opt == "length") {
+ r_ret = get_end_bone_length(which);
+ } else {
+ return false;
+ }
+ } else if (what == "extend_end_bone") {
+ r_ret = is_end_bone_extended(which);
+ } else if (what == "joint_count") {
+ r_ret = get_joint_count(which);
+ } else if (what == "joints") {
+ int idx = path.get_slicec('/', 3).to_int();
+ String prop = path.get_slicec('/', 4);
+ if (prop == "bone_name") {
+ r_ret = get_joint_bone_name(which, idx);
+ } else if (prop == "bone") {
+ r_ret = get_joint_bone(which, idx);
+ } else {
+ return false;
+ }
+ } else {
+ return false;
+ }
+ }
+ return true;
+}
+
+void ChainIK3D::get_property_list(List *p_list) const {
+ String enum_hint;
+ Skeleton3D *skeleton = get_skeleton();
+ if (skeleton) {
+ enum_hint = skeleton->get_concatenated_bone_names();
+ }
+
+ LocalVector props;
+
+ for (uint32_t i = 0; i < settings.size(); i++) {
+ String path = "settings/" + itos(i) + "/";
+ props.push_back(PropertyInfo(Variant::STRING, path + "root_bone_name", PROPERTY_HINT_ENUM_SUGGESTION, enum_hint));
+ props.push_back(PropertyInfo(Variant::INT, path + "root_bone", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_NO_EDITOR));
+ props.push_back(PropertyInfo(Variant::STRING, path + "end_bone_name", PROPERTY_HINT_ENUM_SUGGESTION, enum_hint));
+ props.push_back(PropertyInfo(Variant::INT, path + "end_bone", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_NO_EDITOR));
+ props.push_back(PropertyInfo(Variant::BOOL, path + "extend_end_bone"));
+ props.push_back(PropertyInfo(Variant::INT, path + "end_bone/direction", PROPERTY_HINT_ENUM, SkeletonModifier3D::get_hint_bone_direction()));
+ props.push_back(PropertyInfo(Variant::FLOAT, path + "end_bone/length", PROPERTY_HINT_RANGE, "0,1,0.001,or_greater,suffix:m"));
+ props.push_back(PropertyInfo(Variant::INT, path + "joint_count", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_DEFAULT | PROPERTY_USAGE_ARRAY, "Joints," + path + "joints/,static,const"));
+ for (uint32_t j = 0; j < chain_settings[i]->joints.size(); j++) {
+ String joint_path = path + "joints/" + itos(j) + "/";
+ props.push_back(PropertyInfo(Variant::STRING, joint_path + "bone_name", PROPERTY_HINT_ENUM_SUGGESTION, enum_hint, PROPERTY_USAGE_EDITOR | PROPERTY_USAGE_READ_ONLY));
+ props.push_back(PropertyInfo(Variant::INT, joint_path + "bone", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_NO_EDITOR | PROPERTY_USAGE_READ_ONLY));
+ }
+ }
+
+ for (PropertyInfo &p : props) {
+ _validate_dynamic_prop(p);
+ p_list->push_back(p);
+ }
+}
+
+void ChainIK3D::_validate_dynamic_prop(PropertyInfo &p_property) const {
+ PackedStringArray split = p_property.name.split("/");
+ if (split.size() > 2 && split[0] == "settings") {
+ int which = split[1].to_int();
+
+ // Extended end bone option.
+ bool force_hide = false;
+ if (split[2] == "extend_end_bone" && get_end_bone(which) == -1) {
+ p_property.usage = PROPERTY_USAGE_NONE;
+ force_hide = true;
+ }
+ if (force_hide || (split[2] == "end_bone" && !is_end_bone_extended(which) && split.size() > 3)) {
+ p_property.usage = PROPERTY_USAGE_NONE;
+ }
+ }
+}
+
+// Setting.
+
+void ChainIK3D::set_root_bone_name(int p_index, const String &p_bone_name) {
+ ERR_FAIL_INDEX(p_index, (int)settings.size());
+ chain_settings[p_index]->root_bone.name = p_bone_name;
+ Skeleton3D *sk = get_skeleton();
+ if (sk) {
+ set_root_bone(p_index, sk->find_bone(chain_settings[p_index]->root_bone.name));
+ }
+}
+
+String ChainIK3D::get_root_bone_name(int p_index) const {
+ ERR_FAIL_INDEX_V(p_index, (int)settings.size(), String());
+ return chain_settings[p_index]->root_bone.name;
+}
+
+void ChainIK3D::set_root_bone(int p_index, int p_bone) {
+ ERR_FAIL_INDEX(p_index, (int)settings.size());
+ bool changed = chain_settings[p_index]->root_bone.bone != p_bone;
+ chain_settings[p_index]->root_bone.bone = p_bone;
+ Skeleton3D *sk = get_skeleton();
+ if (sk) {
+ if (chain_settings[p_index]->root_bone.bone <= -1 || chain_settings[p_index]->root_bone.bone >= sk->get_bone_count()) {
+ WARN_PRINT("Root bone index out of range!");
+ chain_settings[p_index]->root_bone.bone = -1;
+ } else {
+ chain_settings[p_index]->root_bone.name = sk->get_bone_name(chain_settings[p_index]->root_bone.bone);
+ }
+ }
+ if (changed) {
+ _update_joints(p_index);
+ }
+}
+
+int ChainIK3D::get_root_bone(int p_index) const {
+ ERR_FAIL_INDEX_V(p_index, (int)settings.size(), -1);
+ return chain_settings[p_index]->root_bone.bone;
+}
+
+void ChainIK3D::set_end_bone_name(int p_index, const String &p_bone_name) {
+ ERR_FAIL_INDEX(p_index, (int)settings.size());
+ chain_settings[p_index]->end_bone.name = p_bone_name;
+ Skeleton3D *sk = get_skeleton();
+ if (sk) {
+ set_end_bone(p_index, sk->find_bone(chain_settings[p_index]->end_bone.name));
+ }
+}
+
+String ChainIK3D::get_end_bone_name(int p_index) const {
+ ERR_FAIL_INDEX_V(p_index, (int)settings.size(), String());
+ return chain_settings[p_index]->end_bone.name;
+}
+
+void ChainIK3D::set_end_bone(int p_index, int p_bone) {
+ ERR_FAIL_INDEX(p_index, (int)settings.size());
+ bool changed = chain_settings[p_index]->end_bone.bone != p_bone;
+ chain_settings[p_index]->end_bone.bone = p_bone;
+ Skeleton3D *sk = get_skeleton();
+ if (sk) {
+ if (chain_settings[p_index]->end_bone.bone <= -1 || chain_settings[p_index]->end_bone.bone >= sk->get_bone_count()) {
+ WARN_PRINT("End bone index out of range!");
+ chain_settings[p_index]->end_bone.bone = -1;
+ } else {
+ chain_settings[p_index]->end_bone.name = sk->get_bone_name(chain_settings[p_index]->end_bone.bone);
+ }
+ }
+ if (changed) {
+ _update_joints(p_index);
+ }
+ notify_property_list_changed();
+}
+
+int ChainIK3D::get_end_bone(int p_index) const {
+ ERR_FAIL_INDEX_V(p_index, (int)settings.size(), -1);
+ return chain_settings[p_index]->end_bone.bone;
+}
+
+void ChainIK3D::set_extend_end_bone(int p_index, bool p_enabled) {
+ ERR_FAIL_INDEX(p_index, (int)settings.size());
+ chain_settings[p_index]->extend_end_bone = p_enabled;
+ _make_simulation_dirty(p_index);
+ Skeleton3D *sk = get_skeleton();
+ if (sk && !chain_settings[p_index]->joints.is_empty()) {
+ _validate_axis(sk, p_index, chain_settings[p_index]->joints.size() - 1);
+ }
+ notify_property_list_changed();
+#ifdef TOOLS_ENABLED
+ update_gizmos();
+#endif // TOOLS_ENABLED
+}
+
+bool ChainIK3D::is_end_bone_extended(int p_index) const {
+ ERR_FAIL_INDEX_V(p_index, (int)settings.size(), false);
+ return chain_settings[p_index]->extend_end_bone;
+}
+
+void ChainIK3D::set_end_bone_direction(int p_index, BoneDirection p_bone_direction) {
+ ERR_FAIL_INDEX(p_index, (int)settings.size());
+ chain_settings[p_index]->end_bone_direction = p_bone_direction;
+ _make_simulation_dirty(p_index);
+ Skeleton3D *sk = get_skeleton();
+ if (sk && !chain_settings[p_index]->joints.is_empty()) {
+ _validate_axis(sk, p_index, chain_settings[p_index]->joints.size() - 1);
+ }
+#ifdef TOOLS_ENABLED
+ update_gizmos();
+#endif // TOOLS_ENABLED
+}
+
+SkeletonModifier3D::BoneDirection ChainIK3D::get_end_bone_direction(int p_index) const {
+ ERR_FAIL_INDEX_V(p_index, (int)settings.size(), BONE_DIRECTION_FROM_PARENT);
+ return chain_settings[p_index]->end_bone_direction;
+}
+
+void ChainIK3D::set_end_bone_length(int p_index, float p_length) {
+ ERR_FAIL_INDEX(p_index, (int)settings.size());
+ chain_settings[p_index]->end_bone_length = p_length;
+ _make_simulation_dirty(p_index);
+#ifdef TOOLS_ENABLED
+ update_gizmos();
+#endif // TOOLS_ENABLED
+}
+
+float ChainIK3D::get_end_bone_length(int p_index) const {
+ ERR_FAIL_INDEX_V(p_index, (int)settings.size(), 0);
+ return chain_settings[p_index]->end_bone_length;
+}
+
+// Individual joints.
+
+void ChainIK3D::set_joint_bone_name(int p_index, int p_joint, const String &p_bone_name) {
+ // Exists only for indicate bone name on the inspector, no needs to make dirty joint array.
+ ERR_FAIL_INDEX(p_index, (int)settings.size());
+ LocalVector &joints = chain_settings[p_index]->joints;
+ ERR_FAIL_INDEX(p_joint, (int)joints.size());
+ joints[p_joint].name = p_bone_name;
+ Skeleton3D *sk = get_skeleton();
+ if (sk) {
+ set_joint_bone(p_index, p_joint, sk->find_bone(joints[p_joint].name));
+ }
+}
+
+String ChainIK3D::get_joint_bone_name(int p_index, int p_joint) const {
+ ERR_FAIL_INDEX_V(p_index, (int)settings.size(), String());
+ const LocalVector &joints = chain_settings[p_index]->joints;
+ ERR_FAIL_INDEX_V(p_joint, (int)joints.size(), String());
+ return joints[p_joint].name;
+}
+
+void ChainIK3D::set_joint_bone(int p_index, int p_joint, int p_bone) {
+ ERR_FAIL_INDEX(p_index, (int)settings.size());
+ LocalVector &joints = chain_settings[p_index]->joints;
+ ERR_FAIL_INDEX(p_joint, (int)joints.size());
+ joints[p_joint].bone = p_bone;
+ Skeleton3D *sk = get_skeleton();
+ if (sk) {
+ if (joints[p_joint].bone <= -1 || joints[p_joint].bone >= sk->get_bone_count()) {
+ WARN_PRINT("Joint bone index out of range!");
+ joints[p_joint].bone = -1;
+ } else {
+ joints[p_joint].name = sk->get_bone_name(joints[p_joint].bone);
+ }
+ }
+}
+
+int ChainIK3D::get_joint_bone(int p_index, int p_joint) const {
+ ERR_FAIL_INDEX_V(p_index, (int)settings.size(), -1);
+ const LocalVector &joints = chain_settings[p_index]->joints;
+ ERR_FAIL_INDEX_V(p_joint, (int)joints.size(), -1);
+ return joints[p_joint].bone;
+}
+
+void ChainIK3D::set_joint_count(int p_index, int p_count) {
+ ERR_FAIL_INDEX(p_index, (int)settings.size());
+ ERR_FAIL_COND(p_count < 0);
+ LocalVector &joints = chain_settings[p_index]->joints;
+ joints.resize(p_count);
+ _set_joint_count(p_index, p_count);
+ notify_property_list_changed();
+}
+
+void ChainIK3D::_set_joint_count(int p_index, int p_count) {
+ //
+}
+
+int ChainIK3D::get_joint_count(int p_index) const {
+ ERR_FAIL_INDEX_V(p_index, (int)settings.size(), 0);
+ const LocalVector &joints = chain_settings[p_index]->joints;
+ return joints.size();
+}
+
+void ChainIK3D::_bind_methods() {
+ // Setting.
+ ClassDB::bind_method(D_METHOD("set_root_bone_name", "index", "bone_name"), &ChainIK3D::set_root_bone_name);
+ ClassDB::bind_method(D_METHOD("get_root_bone_name", "index"), &ChainIK3D::get_root_bone_name);
+ ClassDB::bind_method(D_METHOD("set_root_bone", "index", "bone"), &ChainIK3D::set_root_bone);
+ ClassDB::bind_method(D_METHOD("get_root_bone", "index"), &ChainIK3D::get_root_bone);
+
+ ClassDB::bind_method(D_METHOD("set_end_bone_name", "index", "bone_name"), &ChainIK3D::set_end_bone_name);
+ ClassDB::bind_method(D_METHOD("get_end_bone_name", "index"), &ChainIK3D::get_end_bone_name);
+ ClassDB::bind_method(D_METHOD("set_end_bone", "index", "bone"), &ChainIK3D::set_end_bone);
+ ClassDB::bind_method(D_METHOD("get_end_bone", "index"), &ChainIK3D::get_end_bone);
+
+ ClassDB::bind_method(D_METHOD("set_extend_end_bone", "index", "enabled"), &ChainIK3D::set_extend_end_bone);
+ ClassDB::bind_method(D_METHOD("is_end_bone_extended", "index"), &ChainIK3D::is_end_bone_extended);
+ ClassDB::bind_method(D_METHOD("set_end_bone_direction", "index", "bone_direction"), &ChainIK3D::set_end_bone_direction);
+ ClassDB::bind_method(D_METHOD("get_end_bone_direction", "index"), &ChainIK3D::get_end_bone_direction);
+ ClassDB::bind_method(D_METHOD("set_end_bone_length", "index", "length"), &ChainIK3D::set_end_bone_length);
+ ClassDB::bind_method(D_METHOD("get_end_bone_length", "index"), &ChainIK3D::get_end_bone_length);
+
+ // Individual joints.
+ ClassDB::bind_method(D_METHOD("get_joint_bone_name", "index", "joint"), &ChainIK3D::get_joint_bone_name);
+ ClassDB::bind_method(D_METHOD("get_joint_bone", "index", "joint"), &ChainIK3D::get_joint_bone);
+
+ ClassDB::bind_method(D_METHOD("get_joint_count", "index"), &ChainIK3D::get_joint_count);
+}
+
+void ChainIK3D::_validate_bone_names() {
+ for (uint32_t i = 0; i < settings.size(); i++) {
+ // Prior bone name.
+ if (!chain_settings[i]->root_bone.name.is_empty()) {
+ set_root_bone_name(i, chain_settings[i]->root_bone.name);
+ } else if (chain_settings[i]->root_bone.bone != -1) {
+ set_root_bone(i, chain_settings[i]->root_bone.bone);
+ }
+ // Prior bone name.
+ if (!chain_settings[i]->end_bone.name.is_empty()) {
+ set_end_bone_name(i, chain_settings[i]->end_bone.name);
+ } else if (chain_settings[i]->end_bone.bone != -1) {
+ set_end_bone(i, chain_settings[i]->end_bone.bone);
+ }
+ }
+}
+
+void ChainIK3D::_validate_axes(Skeleton3D *p_skeleton) const {
+ for (uint32_t i = 0; i < settings.size(); i++) {
+ for (uint32_t j = 0; j < chain_settings[i]->joints.size(); j++) {
+ _validate_axis(p_skeleton, i, j);
+ }
+ }
+}
+
+void ChainIK3D::_validate_axis(Skeleton3D *p_skeleton, int p_index, int p_joint) const {
+ //
+}
+
+void ChainIK3D::_make_all_joints_dirty() {
+ for (uint32_t i = 0; i < settings.size(); i++) {
+ _update_joints(i);
+ }
+}
+
+void ChainIK3D::_update_joints(int p_index) {
+ _make_simulation_dirty(p_index);
+
+#ifdef TOOLS_ENABLED
+ update_gizmos(); // To clear invalid setting.
+#endif // TOOLS_ENABLED
+
+ Skeleton3D *sk = get_skeleton();
+ int current_bone = chain_settings[p_index]->end_bone.bone;
+ int root_bone = chain_settings[p_index]->root_bone.bone;
+ if (!sk || current_bone < 0 || root_bone < 0) {
+ set_joint_count(p_index, 0);
+ return;
+ }
+
+ // Validation.
+ bool valid = false;
+ while (current_bone >= 0) {
+ if (current_bone == root_bone) {
+ valid = true;
+ break;
+ }
+ current_bone = sk->get_bone_parent(current_bone);
+ }
+
+ if (!valid) {
+ set_joint_count(p_index, 0);
+ ERR_FAIL_EDMSG("End bone must be the same as or a child of the root bone.");
+ }
+
+ Vector new_joints;
+ current_bone = chain_settings[p_index]->end_bone.bone;
+ while (current_bone != root_bone) {
+ new_joints.push_back(current_bone);
+ current_bone = sk->get_bone_parent(current_bone);
+ }
+ new_joints.push_back(current_bone);
+ new_joints.reverse();
+
+ set_joint_count(p_index, new_joints.size());
+ for (uint32_t i = 0; i < new_joints.size(); i++) {
+ set_joint_bone(p_index, i, new_joints[i]);
+ }
+
+ if (sk) {
+ _validate_axes(sk);
+ }
+
+#ifdef TOOLS_ENABLED
+ update_gizmos();
+#endif // TOOLS_ENABLED
+}
+
+void ChainIK3D::_process_ik(Skeleton3D *p_skeleton, double p_delta) {
+ //
+}
+
+ChainIK3D::~ChainIK3D() {
+ clear_settings();
+}
diff --git a/scene/3d/chain_ik_3d.h b/scene/3d/chain_ik_3d.h
new file mode 100644
index 00000000000..0209c324c63
--- /dev/null
+++ b/scene/3d/chain_ik_3d.h
@@ -0,0 +1,243 @@
+/**************************************************************************/
+/* chain_ik_3d.h */
+/**************************************************************************/
+/* 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. */
+/**************************************************************************/
+
+#pragma once
+
+#include "scene/3d/ik_modifier_3d.h"
+
+class ChainIK3D : public IKModifier3D {
+ GDCLASS(ChainIK3D, IKModifier3D);
+
+public:
+ struct ChainIK3DSetting : public IKModifier3DSetting {
+ BoneJoint root_bone;
+ BoneJoint end_bone;
+
+ // To make virtual end joint.
+ bool extend_end_bone = false;
+ BoneDirection end_bone_direction = BONE_DIRECTION_FROM_PARENT;
+ float end_bone_length = 0.0;
+
+ LocalVector joints;
+ LocalVector solver_info_list;
+ LocalVector chain;
+
+ // Only update chain coordinates to avoid to override previous result (bone poses).
+ // Chain coordinates will be converted to bone pose by child class cache_current_joint_rotations() in the end of iterating.
+ void update_chain_coordinate(Skeleton3D *p_skeleton, int p_index, const Vector3 &p_position) {
+ // Don't update if the position is same as the current position ().
+ // But distance_squared_to() is unsuitable because converting position to rotation requires a certain level of precision.
+ if (Math::is_zero_approx(chain[p_index].distance_to(p_position))) {
+ return;
+ }
+
+ // Allow flipping.
+ chain[p_index] = p_position;
+ cache_current_vector(p_skeleton, p_index);
+ }
+
+ void update_chain_coordinate_bw(Skeleton3D *p_skeleton, int p_index, const Vector3 &p_position) {
+ // Don't update if the position is same as the current position.
+ // But distance_squared_to() is unsuitable because converting position to rotation requires a certain level of precision.
+ if (Math::is_zero_approx(chain[p_index].distance_to(p_position))) {
+ return;
+ }
+
+ // Prevent flipping from backwards.
+ Vector3 result = p_position;
+ int HEAD = p_index - 1;
+ int TAIL = p_index;
+ if (HEAD >= 0 && HEAD < (int)solver_info_list.size()) {
+ IKModifier3DSolverInfo *solver_info = solver_info_list[HEAD];
+ if (solver_info) {
+ Vector3 old_head_to_tail = solver_info->current_vector;
+ Vector3 new_head_to_tail = (result - chain[HEAD]).normalized();
+ if (Math::is_equal_approx((double)old_head_to_tail.dot(new_head_to_tail), -1.0)) {
+ chain[TAIL] = chain[HEAD] + old_head_to_tail * solver_info->length; // Revert.
+ return; // No change, cache is not updated.
+ }
+ }
+ }
+
+ chain[p_index] = result;
+ cache_current_vector(p_skeleton, p_index);
+ }
+
+ void update_chain_coordinate_fw(Skeleton3D *p_skeleton, int p_index, const Vector3 &p_position) {
+ // Don't update if the position is same as the current position.
+ // But distance_squared_to() is unsuitable because converting position to rotation requires a certain level of precision.
+ if (Math::is_zero_approx(chain[p_index].distance_to(p_position))) {
+ return;
+ }
+
+ // Prevent flipping from forwards.
+ Vector3 result = p_position;
+ int HEAD = p_index;
+ int TAIL = p_index + 1;
+ if (TAIL >= 0 && TAIL < (int)solver_info_list.size()) {
+ IKModifier3DSolverInfo *solver_info = solver_info_list[HEAD];
+ if (solver_info) {
+ Vector3 old_head_to_tail = solver_info->current_vector;
+ Vector3 new_head_to_tail = (chain[TAIL] - result).normalized();
+ if (Math::is_equal_approx((double)old_head_to_tail.dot(new_head_to_tail), -1.0)) {
+ chain[HEAD] = chain[TAIL] - old_head_to_tail * solver_info->length; // Revert.
+ return; // No change, cache is not updated.
+ }
+ }
+ }
+
+ chain[p_index] = result;
+ cache_current_vector(p_skeleton, p_index);
+ }
+
+ void cache_current_vector(Skeleton3D *p_skeleton, int p_index) {
+ int cur_head = p_index - 1;
+ int cur_tail = p_index;
+ if (cur_head >= 0) {
+ solver_info_list[cur_head]->current_vector = (chain[cur_tail] - chain[cur_head]).normalized();
+ }
+ cur_head = p_index;
+ cur_tail = p_index + 1;
+ if (cur_tail < (int)chain.size()) {
+ solver_info_list[cur_head]->current_vector = (chain[cur_tail] - chain[cur_head]).normalized();
+ }
+ }
+
+ void cache_current_vectors(Skeleton3D *p_skeleton) {
+ for (uint32_t i = 0; i < joints.size(); i++) {
+ int HEAD = i;
+ int TAIL = i + 1;
+ IKModifier3DSolverInfo *solver_info = solver_info_list[HEAD];
+ if (!solver_info) {
+ continue;
+ }
+ solver_info->current_vector = (chain[TAIL] - chain[HEAD]).normalized();
+ }
+ }
+
+ void init_current_joint_rotations(Skeleton3D *p_skeleton) {
+ if (root_bone.bone < 0) {
+ return;
+ }
+
+ Quaternion parent_gpose;
+ int parent = p_skeleton->get_bone_parent(root_bone.bone);
+ if (parent >= 0) {
+ parent_gpose = p_skeleton->get_bone_global_pose(parent).basis.get_rotation_quaternion();
+ }
+
+ for (uint32_t i = 0; i < joints.size(); i++) {
+ IKModifier3DSolverInfo *solver_info = solver_info_list[i];
+ if (!solver_info) {
+ continue;
+ }
+ solver_info->current_lrest = p_skeleton->get_bone_pose(joints[i].bone).basis.get_rotation_quaternion();
+ solver_info->current_grest = parent_gpose * solver_info->current_lrest;
+ solver_info->current_grest.normalize();
+ solver_info->current_lpose = p_skeleton->get_bone_pose(joints[i].bone).basis.get_rotation_quaternion();
+ solver_info->current_gpose = parent_gpose * solver_info->current_lpose;
+ solver_info->current_gpose.normalize();
+ parent_gpose = solver_info->current_gpose;
+ }
+
+ cache_current_vectors(p_skeleton);
+ }
+
+ ~ChainIK3DSetting() {
+ for (uint32_t i = 0; i < solver_info_list.size(); i++) {
+ if (solver_info_list[i]) {
+ memdelete(solver_info_list[i]);
+ solver_info_list[i] = nullptr;
+ }
+ }
+ solver_info_list.clear();
+ }
+ };
+
+protected:
+ LocalVector chain_settings; // For caching.
+
+ bool _get(const StringName &p_path, Variant &r_ret) const;
+ bool _set(const StringName &p_path, const Variant &p_value);
+ void get_property_list(List *p_list) const;
+ void _validate_dynamic_prop(PropertyInfo &p_property) const;
+
+ static void _bind_methods();
+
+ virtual void _validate_bone_names() override;
+ void _validate_axes(Skeleton3D *p_skeleton) const;
+ virtual void _validate_axis(Skeleton3D *p_skeleton, int p_index, int p_joint) const;
+
+ virtual void _make_all_joints_dirty() override;
+ virtual void _update_joints(int p_index) override;
+
+ virtual void _process_ik(Skeleton3D *p_skeleton, double p_delta) override;
+
+ virtual void _set_joint_count(int p_index, int p_count);
+
+public:
+ virtual void set_setting_count(int p_count) override {
+ _set_setting_count(p_count);
+ chain_settings = _cast_settings();
+ }
+ virtual void clear_settings() override {
+ _set_setting_count(0);
+ chain_settings.clear();
+ }
+
+ // Setting.
+ void set_root_bone_name(int p_index, const String &p_bone_name);
+ String get_root_bone_name(int p_index) const;
+ void set_root_bone(int p_index, int p_bone);
+ int get_root_bone(int p_index) const;
+
+ void set_end_bone_name(int p_index, const String &p_bone_name);
+ String get_end_bone_name(int p_index) const;
+ void set_end_bone(int p_index, int p_bone);
+ int get_end_bone(int p_index) const;
+
+ void set_extend_end_bone(int p_index, bool p_enabled);
+ bool is_end_bone_extended(int p_index) const;
+ void set_end_bone_direction(int p_index, BoneDirection p_bone_direction);
+ BoneDirection get_end_bone_direction(int p_index) const;
+ void set_end_bone_length(int p_index, float p_length);
+ float get_end_bone_length(int p_index) const;
+
+ // Individual joints.
+ void set_joint_bone_name(int p_index, int p_joint, const String &p_bone_name);
+ String get_joint_bone_name(int p_index, int p_joint) const;
+ void set_joint_bone(int p_index, int p_joint, int p_bone);
+ int get_joint_bone(int p_index, int p_joint) const;
+
+ void set_joint_count(int p_index, int p_count);
+ int get_joint_count(int p_index) const;
+
+ ~ChainIK3D();
+};
diff --git a/scene/3d/fabr_ik_3d.cpp b/scene/3d/fabr_ik_3d.cpp
new file mode 100644
index 00000000000..3707954957c
--- /dev/null
+++ b/scene/3d/fabr_ik_3d.cpp
@@ -0,0 +1,87 @@
+/**************************************************************************/
+/* fabr_ik_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 "fabr_ik_3d.h"
+
+void FABRIK3D::_solve_iteration(double p_delta, Skeleton3D *p_skeleton, IterateIK3DSetting *p_setting, const Vector3 &p_destination) {
+ int joint_size = (int)p_setting->joints.size();
+
+ // Backwards.
+ bool first = true;
+ for (int i = joint_size - 1; i >= 0; i--) {
+ IKModifier3DSolverInfo *solver_info = p_setting->solver_info_list[i];
+ if (!solver_info || Math::is_zero_approx(solver_info->length)) {
+ continue;
+ }
+
+ int HEAD = i;
+ int TAIL = i + 1;
+
+ if (first) {
+ p_setting->update_chain_coordinate_bw(p_skeleton, TAIL, p_destination);
+ first = false;
+ }
+
+ p_setting->update_chain_coordinate_bw(p_skeleton, HEAD, limit_length(p_setting->chain[TAIL], p_setting->chain[HEAD], solver_info->length));
+
+ if (p_setting->joint_settings[HEAD]->rotation_axis != ROTATION_AXIS_ALL) {
+ p_setting->update_chain_coordinate_bw(p_skeleton, HEAD, p_setting->chain[TAIL] + p_setting->joint_settings[HEAD]->get_projected_rotation(solver_info->current_grest, p_setting->chain[HEAD] - p_setting->chain[TAIL]));
+ }
+ if (p_setting->joint_settings[HEAD]->limitation.is_valid()) {
+ p_setting->update_chain_coordinate_bw(p_skeleton, HEAD, p_setting->chain[TAIL] + p_setting->joint_settings[HEAD]->get_limited_rotation(solver_info->current_grest, p_setting->chain[HEAD] - p_setting->chain[TAIL], solver_info->forward_vector));
+ }
+ }
+
+ // Forwards.
+ first = true;
+ for (int i = 0; i < joint_size; i++) {
+ IKModifier3DSolverInfo *solver_info = p_setting->solver_info_list[i];
+ if (!solver_info || Math::is_zero_approx(solver_info->length)) {
+ continue;
+ }
+
+ int HEAD = i;
+ int TAIL = i + 1;
+
+ if (first) {
+ p_setting->update_chain_coordinate_fw(p_skeleton, HEAD, p_skeleton->get_bone_global_pose(p_setting->joints[HEAD].bone).origin);
+ first = false;
+ }
+
+ p_setting->update_chain_coordinate_fw(p_skeleton, TAIL, limit_length(p_setting->chain[HEAD], p_setting->chain[TAIL], solver_info->length));
+
+ if (p_setting->joint_settings[HEAD]->rotation_axis != ROTATION_AXIS_ALL) {
+ p_setting->update_chain_coordinate_fw(p_skeleton, TAIL, p_setting->chain[HEAD] + p_setting->joint_settings[HEAD]->get_projected_rotation(solver_info->current_grest, p_setting->chain[TAIL] - p_setting->chain[HEAD]));
+ }
+ if (p_setting->joint_settings[HEAD]->limitation.is_valid()) {
+ p_setting->update_chain_coordinate_fw(p_skeleton, TAIL, p_setting->chain[HEAD] + p_setting->joint_settings[HEAD]->get_limited_rotation(solver_info->current_grest, p_setting->chain[TAIL] - p_setting->chain[HEAD], solver_info->forward_vector));
+ }
+ }
+}
diff --git a/scene/3d/fabr_ik_3d.h b/scene/3d/fabr_ik_3d.h
new file mode 100644
index 00000000000..6e8415942db
--- /dev/null
+++ b/scene/3d/fabr_ik_3d.h
@@ -0,0 +1,40 @@
+/**************************************************************************/
+/* fabr_ik_3d.h */
+/**************************************************************************/
+/* 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. */
+/**************************************************************************/
+
+#pragma once
+
+#include "scene/3d/iterate_ik_3d.h"
+
+class FABRIK3D : public IterateIK3D {
+ GDCLASS(FABRIK3D, IterateIK3D);
+
+protected:
+ virtual void _solve_iteration(double p_delta, Skeleton3D *p_skeleton, IterateIK3DSetting *p_setting, const Vector3 &p_destination) override;
+};
diff --git a/scene/3d/ik_modifier_3d.cpp b/scene/3d/ik_modifier_3d.cpp
new file mode 100644
index 00000000000..c0ca377b5dc
--- /dev/null
+++ b/scene/3d/ik_modifier_3d.cpp
@@ -0,0 +1,151 @@
+/**************************************************************************/
+/* ik_modifier_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 "ik_modifier_3d.h"
+
+void IKModifier3D::_notification(int p_what) {
+ switch (p_what) {
+ case NOTIFICATION_ENTER_TREE: {
+#ifdef TOOLS_ENABLED
+ if (Engine::get_singleton()->is_editor_hint()) {
+ set_notify_local_transform(true); // Used for updating gizmo in editor.
+ }
+#endif // TOOLS_ENABLED
+ _make_all_joints_dirty();
+ } break;
+#ifdef TOOLS_ENABLED
+ case NOTIFICATION_LOCAL_TRANSFORM_CHANGED: {
+ update_gizmos();
+ } break;
+#endif // TOOLS_ENABLED
+ }
+}
+
+void IKModifier3D::_bind_methods() {
+ ClassDB::bind_method(D_METHOD("set_setting_count", "count"), &IKModifier3D::set_setting_count);
+ ClassDB::bind_method(D_METHOD("get_setting_count"), &IKModifier3D::get_setting_count);
+ ClassDB::bind_method(D_METHOD("clear_settings"), &IKModifier3D::clear_settings);
+
+ // To process manually.
+ ClassDB::bind_method(D_METHOD("reset"), &IKModifier3D::reset);
+}
+
+void IKModifier3D::_set_active(bool p_active) {
+ if (p_active) {
+ reset();
+ }
+}
+
+void IKModifier3D::_skeleton_changed(Skeleton3D *p_old, Skeleton3D *p_new) {
+ if (p_old && p_old->is_connected(SNAME("rest_updated"), callable_mp(this, &IKModifier3D::_make_all_joints_dirty))) {
+ p_old->disconnect(SNAME("rest_updated"), callable_mp(this, &IKModifier3D::_make_all_joints_dirty));
+ }
+ if (p_new && !p_new->is_connected(SNAME("rest_updated"), callable_mp(this, &IKModifier3D::_make_all_joints_dirty))) {
+ p_new->connect(SNAME("rest_updated"), callable_mp(this, &IKModifier3D::_make_all_joints_dirty));
+ }
+ _make_all_joints_dirty();
+}
+
+void IKModifier3D::_validate_bone_names() {
+ //
+}
+
+void IKModifier3D::_make_all_joints_dirty() {
+ //
+}
+
+void IKModifier3D::_init_joints(Skeleton3D *p_skeleton, int p_index) {
+ //
+}
+
+void IKModifier3D::_update_joints(int p_index) {
+ //
+}
+
+void IKModifier3D::_make_simulation_dirty(int p_index) {
+ //
+}
+
+void IKModifier3D::_process_modification(double p_delta) {
+ Skeleton3D *skeleton = get_skeleton();
+ if (!skeleton) {
+ return;
+ }
+
+ _process_ik(skeleton, p_delta);
+}
+
+void IKModifier3D::_process_ik(Skeleton3D *p_skeleton, double p_delta) {
+ //
+}
+
+Quaternion IKModifier3D::get_local_pose_rotation(Skeleton3D *p_skeleton, int p_bone, const Quaternion &p_global_pose_rotation) {
+ int parent = p_skeleton->get_bone_parent(p_bone);
+ if (parent < 0) {
+ return p_global_pose_rotation;
+ }
+ return p_skeleton->get_bone_global_pose(parent).basis.get_rotation_quaternion().inverse() * p_global_pose_rotation;
+}
+
+Vector3 IKModifier3D::get_bone_axis(int p_end_bone, BoneDirection p_direction) const {
+ if (!is_inside_tree()) {
+ return Vector3();
+ }
+ Vector3 axis;
+ if (p_direction == BONE_DIRECTION_FROM_PARENT) {
+ Skeleton3D *sk = get_skeleton();
+ if (sk) {
+ axis = sk->get_bone_rest(p_end_bone).basis.xform_inv(sk->get_bone_rest(p_end_bone).origin);
+ axis.normalize();
+ }
+ } else {
+ axis = get_vector_from_bone_axis(static_cast((int)p_direction));
+ }
+ return axis;
+}
+
+int IKModifier3D::get_setting_count() const {
+ return settings.size();
+}
+
+void IKModifier3D::reset() {
+ Skeleton3D *skeleton = get_skeleton();
+ if (!skeleton) {
+ return;
+ }
+ for (uint32_t i = 0; i < settings.size(); i++) {
+ _make_simulation_dirty(i);
+ _init_joints(skeleton, i);
+ }
+}
+
+IKModifier3D::~IKModifier3D() {
+ clear_settings();
+}
diff --git a/scene/3d/ik_modifier_3d.h b/scene/3d/ik_modifier_3d.h
new file mode 100644
index 00000000000..2b9fa552dbf
--- /dev/null
+++ b/scene/3d/ik_modifier_3d.h
@@ -0,0 +1,132 @@
+/**************************************************************************/
+/* ik_modifier_3d.h */
+/**************************************************************************/
+/* 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. */
+/**************************************************************************/
+
+#pragma once
+
+#include "scene/3d/skeleton_modifier_3d.h"
+
+class IKModifier3D : public SkeletonModifier3D {
+ GDCLASS(IKModifier3D, SkeletonModifier3D);
+
+protected:
+#ifdef TOOLS_ENABLED
+ bool saving = false;
+#endif // TOOLS_ENABLED
+
+ Transform3D cached_space;
+ bool joints_dirty = false;
+
+public:
+ struct BoneJoint {
+ StringName name;
+ int bone = -1;
+ };
+
+ struct IKModifier3DSolverInfo {
+ Quaternion current_lpose;
+ Quaternion current_lrest;
+ Quaternion current_gpose;
+ Quaternion current_grest;
+ Vector3 current_vector; // Global so needs xfrom_inv by gpose or grest in the process.
+ Vector3 forward_vector; // Local.
+ float length = 0.0;
+ };
+
+ struct IKModifier3DSetting {
+ bool simulation_dirty = true;
+ bool joints_dirty = false;
+ };
+
+protected:
+ LocalVector settings;
+
+ void _notification(int p_what);
+ static void _bind_methods();
+
+ virtual void _set_active(bool p_active) override;
+ virtual void _skeleton_changed(Skeleton3D *p_old, Skeleton3D *p_new) override;
+
+ virtual void _validate_bone_names() override;
+
+ virtual void _make_all_joints_dirty();
+ virtual void _init_joints(Skeleton3D *p_skeleton, int p_index);
+ virtual void _update_joints(int p_index);
+ virtual void _make_simulation_dirty(int p_index);
+
+ virtual void _process_modification(double p_delta) override;
+ virtual void _process_ik(Skeleton3D *p_skeleton, double p_delta);
+
+ template
+ void _set_setting_count(int p_count) {
+ ERR_FAIL_COND(p_count < 0);
+ int delta = p_count - settings.size();
+ if (delta < 0) {
+ for (int i = delta; i < 0; i++) {
+ memdelete(static_cast(settings[settings.size() + i]));
+ settings[settings.size() + i] = nullptr;
+ }
+ }
+ settings.resize(p_count);
+ delta++;
+ if (delta > 1) {
+ for (int i = 1; i < delta; i++) {
+ settings[p_count - i] = memnew(T);
+ }
+ }
+ notify_property_list_changed();
+ }
+ template
+ LocalVector _cast_settings() const {
+ LocalVector result;
+ for (uint32_t i = 0; i < settings.size(); i++) {
+ result.push_back(static_cast(settings[i]));
+ }
+ return result;
+ }
+
+public:
+ int get_setting_count() const;
+
+ virtual void set_setting_count(int p_count) {
+ _set_setting_count(p_count);
+ }
+ virtual void clear_settings() {
+ _set_setting_count(0);
+ }
+
+ // Helper.
+ static Quaternion get_local_pose_rotation(Skeleton3D *p_skeleton, int p_bone, const Quaternion &p_global_pose_rotation);
+ Vector3 get_bone_axis(int p_end_bone, BoneDirection p_direction) const;
+
+ // To process manually.
+ void reset();
+
+ ~IKModifier3D();
+};
diff --git a/scene/3d/iterate_ik_3d.cpp b/scene/3d/iterate_ik_3d.cpp
new file mode 100644
index 00000000000..e49b7165405
--- /dev/null
+++ b/scene/3d/iterate_ik_3d.cpp
@@ -0,0 +1,554 @@
+/**************************************************************************/
+/* iterate_ik_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 "iterate_ik_3d.h"
+
+bool IterateIK3D::_set(const StringName &p_path, const Variant &p_value) {
+ String path = p_path;
+
+ if (path.begins_with("settings/")) {
+ int which = path.get_slicec('/', 1).to_int();
+ String what = path.get_slicec('/', 2);
+ ERR_FAIL_INDEX_V(which, (int)settings.size(), false);
+
+ if (what == "target_node") {
+ set_target_node(which, p_value);
+ } else if (what == "joints") {
+ int idx = path.get_slicec('/', 3).to_int();
+ String prop = path.get_slicec('/', 4);
+ if (prop == "rotation_axis") {
+ set_joint_rotation_axis(which, idx, static_cast((int)p_value));
+ } else if (prop == "rotation_axis_vector") {
+ set_joint_rotation_axis_vector(which, idx, p_value);
+ } else if (prop == "limitation") {
+ String opt = path.get_slicec('/', 5);
+ if (opt.is_empty()) {
+ set_joint_limitation(which, idx, p_value);
+ } else if (opt == "right_axis") {
+ set_joint_limitation_right_axis(which, idx, p_value);
+ } else if (opt == "right_axis_vector") {
+ set_joint_limitation_right_axis_vector(which, idx, p_value);
+ } else if (opt == "rotation_offset") {
+ set_joint_limitation_rotation_offset(which, idx, p_value);
+ } else {
+ return false;
+ }
+ } else {
+ return false;
+ }
+ } else {
+ return false;
+ }
+ }
+ return true;
+}
+
+bool IterateIK3D::_get(const StringName &p_path, Variant &r_ret) const {
+ String path = p_path;
+
+ if (path.begins_with("settings/")) {
+ int which = path.get_slicec('/', 1).to_int();
+ String what = path.get_slicec('/', 2);
+ ERR_FAIL_INDEX_V(which, (int)settings.size(), false);
+
+ if (what == "target_node") {
+ r_ret = get_target_node(which);
+ } else if (what == "joints") {
+ int idx = path.get_slicec('/', 3).to_int();
+ String prop = path.get_slicec('/', 4);
+ if (prop == "rotation_axis") {
+ r_ret = (int)get_joint_rotation_axis(which, idx);
+ } else if (prop == "rotation_axis_vector") {
+ r_ret = get_joint_rotation_axis_vector(which, idx);
+ } else if (prop == "limitation") {
+ String opt = path.get_slicec('/', 5);
+ if (opt.is_empty()) {
+ r_ret = get_joint_limitation(which, idx);
+ } else if (opt == "right_axis") {
+ r_ret = get_joint_limitation_right_axis(which, idx);
+ } else if (opt == "right_axis_vector") {
+ r_ret = get_joint_limitation_right_axis_vector(which, idx);
+ } else if (opt == "rotation_offset") {
+ r_ret = get_joint_limitation_rotation_offset(which, idx);
+ } else {
+ return false;
+ }
+ } else {
+ return false;
+ }
+ } else {
+ return false;
+ }
+ }
+ return true;
+}
+
+void IterateIK3D::_get_property_list(List *p_list) const {
+ LocalVector props;
+ for (uint32_t i = 0; i < settings.size(); i++) {
+ String path = "settings/" + itos(i) + "/";
+ p_list->push_back(PropertyInfo(Variant::NODE_PATH, path + "target_node"));
+ for (uint32_t j = 0; j < iterate_settings[i]->joints.size(); j++) {
+ String joint_path = path + "joints/" + itos(j) + "/";
+ props.push_back(PropertyInfo(Variant::INT, joint_path + "rotation_axis", PROPERTY_HINT_ENUM, SkeletonModifier3D::get_hint_rotation_axis()));
+ props.push_back(PropertyInfo(Variant::VECTOR3, joint_path + "rotation_axis_vector"));
+ props.push_back(PropertyInfo(Variant::OBJECT, joint_path + "limitation", PROPERTY_HINT_RESOURCE_TYPE, "JointLimitation3D"));
+ props.push_back(PropertyInfo(Variant::INT, joint_path + "limitation/right_axis", PROPERTY_HINT_ENUM, SkeletonModifier3D::get_hint_secondary_direction()));
+ props.push_back(PropertyInfo(Variant::VECTOR3, joint_path + "limitation/right_axis_vector"));
+ props.push_back(PropertyInfo(Variant::QUATERNION, joint_path + "limitation/rotation_offset"));
+ }
+ }
+
+ ChainIK3D::get_property_list(p_list);
+
+ for (PropertyInfo &p : props) {
+ _validate_dynamic_prop(p);
+ p_list->push_back(p);
+ }
+}
+
+void IterateIK3D::_validate_dynamic_prop(PropertyInfo &p_property) const {
+ PackedStringArray split = p_property.name.split("/");
+ if (split.size() > 3 && split[0] == "settings") {
+ int which = split[1].to_int();
+ int joint = split[3].to_int();
+ // Joints option.
+ if (split[2] == "joints" && split.size() > 4) {
+ if (split[4] == "rotation_axis_vector" && get_joint_rotation_axis(which, joint) != ROTATION_AXIS_CUSTOM) {
+ p_property.usage = PROPERTY_USAGE_NONE;
+ }
+ if (split[4] == "limitation" && split.size() > 5) {
+ if (get_joint_limitation(which, joint).is_null()) {
+ p_property.usage = PROPERTY_USAGE_NONE;
+ } else if (split[5] == "right_axis_vector" && get_joint_limitation_right_axis(which, joint) != SECONDARY_DIRECTION_CUSTOM) {
+ p_property.usage = PROPERTY_USAGE_NONE;
+ }
+ }
+ }
+ }
+}
+
+PackedStringArray IterateIK3D::get_configuration_warnings() const {
+ PackedStringArray warnings = SkeletonModifier3D::get_configuration_warnings();
+ for (uint32_t i = 0; i < iterate_settings.size(); i++) {
+ if (iterate_settings[i]->target_node.is_empty()) {
+ warnings.push_back(RTR("Detecting settings with no target set! IterateIK3D must have a target to work."));
+ break;
+ }
+ }
+ return warnings;
+}
+
+void IterateIK3D::set_max_iterations(int p_max_iterations) {
+ max_iterations = p_max_iterations;
+}
+
+int IterateIK3D::get_max_iterations() const {
+ return max_iterations;
+}
+
+void IterateIK3D::set_min_distance(double p_min_distance) {
+ min_distance = p_min_distance;
+}
+
+double IterateIK3D::get_min_distance() const {
+ return min_distance;
+}
+
+void IterateIK3D::set_angular_delta_limit(double p_angular_delta_limit) {
+ angular_delta_limit = p_angular_delta_limit;
+}
+
+double IterateIK3D::get_angular_delta_limit() const {
+ return angular_delta_limit;
+}
+
+// Setting.
+
+void IterateIK3D::set_target_node(int p_index, const NodePath &p_node_path) {
+ ERR_FAIL_INDEX(p_index, (int)settings.size());
+ iterate_settings[p_index]->target_node = p_node_path;
+ update_configuration_warnings();
+}
+
+NodePath IterateIK3D::get_target_node(int p_index) const {
+ ERR_FAIL_INDEX_V(p_index, (int)settings.size(), NodePath());
+ return iterate_settings[p_index]->target_node;
+}
+
+// Individual joints.
+
+void IterateIK3D::set_joint_rotation_axis(int p_index, int p_joint, RotationAxis p_axis) {
+ ERR_FAIL_INDEX(p_index, (int)settings.size());
+ LocalVector &joint_settings = iterate_settings[p_index]->joint_settings;
+ ERR_FAIL_INDEX(p_joint, (int)joint_settings.size());
+ joint_settings[p_joint]->rotation_axis = p_axis;
+ Skeleton3D *sk = get_skeleton();
+ if (sk) {
+ _validate_axis(sk, p_index, p_joint);
+ }
+ notify_property_list_changed();
+ iterate_settings[p_index]->simulation_dirty = true; // Snapping to planes is needed in the initialization, so need to restructure.
+#ifdef TOOLS_ENABLED
+ update_gizmos();
+#endif // TOOLS_ENABLED
+}
+
+SkeletonModifier3D::RotationAxis IterateIK3D::get_joint_rotation_axis(int p_index, int p_joint) const {
+ ERR_FAIL_INDEX_V(p_index, (int)settings.size(), ROTATION_AXIS_ALL);
+ const LocalVector &joint_settings = iterate_settings[p_index]->joint_settings;
+ ERR_FAIL_INDEX_V(p_joint, (int)joint_settings.size(), ROTATION_AXIS_ALL);
+ return joint_settings[p_joint]->rotation_axis;
+}
+
+void IterateIK3D::set_joint_rotation_axis_vector(int p_index, int p_joint, const Vector3 &p_vector) {
+ ERR_FAIL_INDEX(p_index, (int)settings.size());
+ LocalVector &joint_settings = iterate_settings[p_index]->joint_settings;
+ ERR_FAIL_INDEX(p_joint, (int)joint_settings.size());
+ joint_settings[p_joint]->rotation_axis_vector = p_vector;
+ Skeleton3D *sk = get_skeleton();
+ if (sk) {
+ _validate_axis(sk, p_index, p_joint);
+ }
+ iterate_settings[p_index]->simulation_dirty = true; // Snapping to planes is needed in the initialization, so need to restructure.
+#ifdef TOOLS_ENABLED
+ update_gizmos();
+#endif // TOOLS_ENABLED
+}
+
+Vector3 IterateIK3D::get_joint_rotation_axis_vector(int p_index, int p_joint) const {
+ ERR_FAIL_INDEX_V(p_index, (int)settings.size(), Vector3());
+ const LocalVector &joint_settings = iterate_settings[p_index]->joint_settings;
+ ERR_FAIL_INDEX_V(p_joint, (int)joint_settings.size(), Vector3());
+ return joint_settings[p_joint]->get_rotation_axis_vector();
+}
+
+Quaternion IterateIK3D::get_joint_limitation_space(int p_index, int p_joint, const Vector3 &p_forward) const {
+ ERR_FAIL_INDEX_V(p_index, (int)settings.size(), Quaternion());
+ const LocalVector &joint_settings = iterate_settings[p_index]->joint_settings;
+ ERR_FAIL_INDEX_V(p_joint, (int)joint_settings.size(), Quaternion());
+ return joint_settings[p_joint]->get_limitation_space(p_forward);
+}
+
+void IterateIK3D::set_joint_limitation(int p_index, int p_joint, const Ref &p_limitation) {
+ ERR_FAIL_INDEX(p_index, (int)settings.size());
+ LocalVector &joint_settings = iterate_settings[p_index]->joint_settings;
+ ERR_FAIL_INDEX(p_joint, (int)joint_settings.size());
+ _unbind_joint_limitation(p_index, p_joint);
+ joint_settings[p_joint]->limitation = p_limitation;
+ _bind_joint_limitation(p_index, p_joint);
+ notify_property_list_changed();
+ _update_joint_limitation(p_index, p_joint);
+}
+
+Ref IterateIK3D::get_joint_limitation(int p_index, int p_joint) const {
+ ERR_FAIL_INDEX_V(p_index, (int)settings.size(), Ref());
+ const LocalVector &joint_settings = iterate_settings[p_index]->joint_settings;
+ ERR_FAIL_INDEX_V(p_joint, (int)joint_settings.size(), Ref());
+ return joint_settings[p_joint]->limitation;
+}
+
+void IterateIK3D::set_joint_limitation_right_axis(int p_index, int p_joint, SecondaryDirection p_direction) {
+ ERR_FAIL_INDEX(p_index, (int)settings.size());
+ LocalVector &joint_settings = iterate_settings[p_index]->joint_settings;
+ ERR_FAIL_INDEX(p_joint, (int)joint_settings.size());
+ joint_settings[p_joint]->limitation_right_axis = p_direction;
+ notify_property_list_changed();
+ _update_joint_limitation(p_index, p_joint);
+}
+
+IKModifier3D::SecondaryDirection IterateIK3D::get_joint_limitation_right_axis(int p_index, int p_joint) const {
+ ERR_FAIL_INDEX_V(p_index, (int)settings.size(), SECONDARY_DIRECTION_NONE);
+ const LocalVector &joint_settings = iterate_settings[p_index]->joint_settings;
+ ERR_FAIL_INDEX_V(p_joint, (int)joint_settings.size(), SECONDARY_DIRECTION_NONE);
+ return joint_settings[p_joint]->limitation_right_axis;
+}
+
+void IterateIK3D::set_joint_limitation_right_axis_vector(int p_index, int p_joint, const Vector3 &p_vector) {
+ ERR_FAIL_INDEX(p_index, (int)settings.size());
+ LocalVector &joint_settings = iterate_settings[p_index]->joint_settings;
+ ERR_FAIL_INDEX(p_joint, (int)joint_settings.size());
+ joint_settings[p_joint]->limitation_right_axis_vector = p_vector;
+ _update_joint_limitation(p_index, p_joint);
+}
+
+Vector3 IterateIK3D::get_joint_limitation_right_axis_vector(int p_index, int p_joint) const {
+ ERR_FAIL_INDEX_V(p_index, (int)settings.size(), Vector3());
+ const LocalVector &joint_settings = iterate_settings[p_index]->joint_settings;
+ ERR_FAIL_INDEX_V(p_joint, (int)joint_settings.size(), Vector3());
+ return joint_settings[p_joint]->get_limitation_right_axis_vector();
+}
+
+void IterateIK3D::set_joint_limitation_rotation_offset(int p_index, int p_joint, const Quaternion &p_offset) {
+ ERR_FAIL_INDEX(p_index, (int)settings.size());
+ LocalVector &joint_settings = iterate_settings[p_index]->joint_settings;
+ ERR_FAIL_INDEX(p_joint, (int)joint_settings.size());
+ joint_settings[p_joint]->limitation_rotation_offset = p_offset;
+ _update_joint_limitation(p_index, p_joint);
+}
+
+Quaternion IterateIK3D::get_joint_limitation_rotation_offset(int p_index, int p_joint) const {
+ ERR_FAIL_INDEX_V(p_index, (int)settings.size(), Quaternion());
+ const LocalVector &joint_settings = iterate_settings[p_index]->joint_settings;
+ ERR_FAIL_INDEX_V(p_joint, (int)joint_settings.size(), Quaternion());
+ return joint_settings[p_joint]->limitation_rotation_offset;
+}
+
+void IterateIK3D::_set_joint_count(int p_index, int p_count) {
+ _unbind_joint_limitations(p_index);
+ LocalVector &joint_settings = iterate_settings[p_index]->joint_settings;
+ int delta = p_count - joint_settings.size();
+ if (delta < 0) {
+ for (int i = delta; i < 0; i++) {
+ memdelete(joint_settings[joint_settings.size() + i]);
+ joint_settings[joint_settings.size() + i] = nullptr;
+ }
+ }
+ joint_settings.resize(p_count);
+ delta++;
+ if (delta > 1) {
+ for (int i = 1; i < delta; i++) {
+ joint_settings[p_count - i] = memnew(IterateIK3DJointSetting);
+ }
+ }
+}
+
+void IterateIK3D::_validate_axis(Skeleton3D *p_skeleton, int p_index, int p_joint) const {
+ RotationAxis axis = iterate_settings[p_index]->joint_settings[p_joint]->rotation_axis;
+ if (axis == ROTATION_AXIS_ALL) {
+ return;
+ }
+ Vector3 rot = get_joint_rotation_axis_vector(p_index, p_joint).normalized();
+ Vector3 fwd;
+ if (p_joint < (int)iterate_settings[p_index]->joints.size() - 1) {
+ fwd = p_skeleton->get_bone_rest(iterate_settings[p_index]->joints[p_joint + 1].bone).origin;
+ } else if (iterate_settings[p_index]->extend_end_bone) {
+ fwd = get_bone_axis(iterate_settings[p_index]->end_bone.bone, iterate_settings[p_index]->end_bone_direction);
+ if (fwd.is_zero_approx()) {
+ return;
+ }
+ }
+ fwd.normalize();
+ if (Math::is_equal_approx(Math::abs(rot.dot(fwd)), 1)) {
+ WARN_PRINT_ED("Setting: " + itos(p_index) + " Joint: " + itos(p_joint) + ": Rotation axis and forward vector are colinear. This is not advised as it may cause unwanted rotation.");
+ }
+}
+
+void IterateIK3D::_bind_methods() {
+ ClassDB::bind_method(D_METHOD("set_max_iterations", "max_iterations"), &IterateIK3D::set_max_iterations);
+ ClassDB::bind_method(D_METHOD("get_max_iterations"), &IterateIK3D::get_max_iterations);
+ ClassDB::bind_method(D_METHOD("set_min_distance", "min_distance"), &IterateIK3D::set_min_distance);
+ ClassDB::bind_method(D_METHOD("get_min_distance"), &IterateIK3D::get_min_distance);
+ ClassDB::bind_method(D_METHOD("set_angular_delta_limit", "angular_delta_limit"), &IterateIK3D::set_angular_delta_limit);
+ ClassDB::bind_method(D_METHOD("get_angular_delta_limit"), &IterateIK3D::get_angular_delta_limit);
+
+ // Setting.
+ ClassDB::bind_method(D_METHOD("set_target_node", "index", "target_node"), &IterateIK3D::set_target_node);
+ ClassDB::bind_method(D_METHOD("get_target_node", "index"), &IterateIK3D::get_target_node);
+
+ // Individual joints.
+ ClassDB::bind_method(D_METHOD("set_joint_rotation_axis", "index", "joint", "axis"), &IterateIK3D::set_joint_rotation_axis);
+ ClassDB::bind_method(D_METHOD("get_joint_rotation_axis", "index", "joint"), &IterateIK3D::get_joint_rotation_axis);
+ ClassDB::bind_method(D_METHOD("set_joint_rotation_axis_vector", "index", "joint", "axis_vector"), &IterateIK3D::set_joint_rotation_axis_vector);
+ ClassDB::bind_method(D_METHOD("get_joint_rotation_axis_vector", "index", "joint"), &IterateIK3D::get_joint_rotation_axis_vector);
+ ClassDB::bind_method(D_METHOD("set_joint_limitation", "index", "joint", "limitation"), &IterateIK3D::set_joint_limitation);
+ ClassDB::bind_method(D_METHOD("get_joint_limitation", "index", "joint"), &IterateIK3D::get_joint_limitation);
+ ClassDB::bind_method(D_METHOD("set_joint_limitation_right_axis", "index", "joint", "direction"), &IterateIK3D::set_joint_limitation_right_axis);
+ ClassDB::bind_method(D_METHOD("get_joint_limitation_right_axis", "index", "joint"), &IterateIK3D::get_joint_limitation_right_axis);
+ ClassDB::bind_method(D_METHOD("set_joint_limitation_right_axis_vector", "index", "joint", "vector"), &IterateIK3D::set_joint_limitation_right_axis_vector);
+ ClassDB::bind_method(D_METHOD("get_joint_limitation_right_axis_vector", "index", "joint"), &IterateIK3D::get_joint_limitation_right_axis_vector);
+ ClassDB::bind_method(D_METHOD("set_joint_limitation_rotation_offset", "index", "joint", "offset"), &IterateIK3D::set_joint_limitation_rotation_offset);
+ ClassDB::bind_method(D_METHOD("get_joint_limitation_rotation_offset", "index", "joint"), &IterateIK3D::get_joint_limitation_rotation_offset);
+
+ ADD_PROPERTY(PropertyInfo(Variant::INT, "max_iterations", PROPERTY_HINT_RANGE, "0,100,or_greater"), "set_max_iterations", "get_max_iterations");
+ ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "min_distance", PROPERTY_HINT_RANGE, "0,1,0.001,or_greater"), "set_min_distance", "get_min_distance");
+ ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "angular_delta_limit", PROPERTY_HINT_RANGE, "0,180,0.001,radians_as_degrees"), "set_angular_delta_limit", "get_angular_delta_limit");
+ ADD_ARRAY_COUNT("Settings", "setting_count", "set_setting_count", "get_setting_count", "settings/");
+}
+
+void IterateIK3D::_init_joints(Skeleton3D *p_skeleton, int p_index) {
+ IterateIK3DSetting *setting = iterate_settings[p_index];
+ cached_space = p_skeleton->get_global_transform_interpolated();
+ if (!setting->simulation_dirty) {
+ return;
+ }
+ _unbind_joint_limitations(p_index);
+ for (uint32_t i = 0; i < setting->solver_info_list.size(); i++) {
+ if (setting->solver_info_list[i]) {
+ memdelete(setting->solver_info_list[i]);
+ setting->solver_info_list[i] = nullptr;
+ }
+ }
+ setting->solver_info_list.clear();
+ setting->solver_info_list.resize_initialized(setting->joints.size());
+ setting->chain.clear();
+ bool extend_end_bone = setting->extend_end_bone && setting->end_bone_length > 0;
+ for (uint32_t i = 0; i < setting->joints.size(); i++) {
+ setting->chain.push_back(p_skeleton->get_bone_global_pose(setting->joints[i].bone).origin);
+ bool last = i == setting->joints.size() - 1;
+ if (last && extend_end_bone && setting->end_bone_length > 0) {
+ Vector3 axis = get_bone_axis(setting->end_bone.bone, setting->end_bone_direction);
+ if (axis.is_zero_approx()) {
+ continue;
+ }
+ setting->solver_info_list[i] = memnew(IKModifier3DSolverInfo);
+ setting->solver_info_list[i]->forward_vector = snap_vector_to_plane(setting->joint_settings[i]->get_rotation_axis_vector(), axis.normalized());
+ setting->solver_info_list[i]->length = setting->end_bone_length;
+ setting->chain.push_back(p_skeleton->get_bone_global_pose(setting->joints[i].bone).xform(axis * setting->end_bone_length));
+ } else if (!last) {
+ Vector3 axis = p_skeleton->get_bone_rest(setting->joints[i + 1].bone).origin;
+ if (axis.is_zero_approx()) {
+ continue; // Means always we need to check solver info, but `!solver_info` means that the bone is zero length, so IK should skip it in the all process.
+ }
+ setting->solver_info_list[i] = memnew(IKModifier3DSolverInfo);
+ setting->solver_info_list[i]->forward_vector = snap_vector_to_plane(setting->joint_settings[i]->get_rotation_axis_vector(), axis.normalized());
+ setting->solver_info_list[i]->length = axis.length();
+ }
+ }
+ _bind_joint_limitations(p_index);
+
+ setting->init_current_joint_rotations(p_skeleton);
+
+ setting->simulation_dirty = false;
+ setting->simulated = false;
+}
+
+void IterateIK3D::_make_simulation_dirty(int p_index) {
+ IterateIK3DSetting *setting = iterate_settings[p_index];
+ if (!setting) {
+ return;
+ }
+ setting->simulation_dirty = true;
+}
+
+void IterateIK3D::_process_ik(Skeleton3D *p_skeleton, double p_delta) {
+ min_distance_squared = min_distance * min_distance;
+ for (uint32_t i = 0; i < settings.size(); i++) {
+ _init_joints(p_skeleton, i);
+ Node3D *target = Object::cast_to(get_node_or_null(iterate_settings[i]->target_node));
+ if (!target || iterate_settings[i]->chain.is_empty()) {
+ continue; // Abort.
+ }
+ iterate_settings[i]->cache_current_joint_rotations(p_skeleton); // Iterate over first to detect parent (outside of the chain) bone pose changes.
+
+ Vector3 destination = cached_space.affine_inverse().xform(target->get_global_transform_interpolated().origin);
+ _process_joints(p_delta, p_skeleton, iterate_settings[i], destination);
+ }
+}
+
+void IterateIK3D::_process_joints(double p_delta, Skeleton3D *p_skeleton, IterateIK3DSetting *p_setting, const Vector3 &p_destination) {
+ double distance_to_target_sq = INFINITY;
+ int iteration_count = 0;
+
+ if (p_setting->is_penetrated(p_destination)) {
+ return;
+ }
+
+ // To prevent oscillation, if it has been processed at least once and target was reached, abort iterating.
+ if (p_setting->simulated) {
+ distance_to_target_sq = p_setting->chain[p_setting->chain.size() - 1].distance_squared_to(p_destination);
+ }
+
+ while (distance_to_target_sq > min_distance_squared && iteration_count < max_iterations) {
+ // Solve the IK for this iteration.
+ _solve_iteration(p_delta, p_skeleton, p_setting, p_destination);
+
+ // Update virtual bone rest/poses.
+ p_setting->cache_current_joint_rotations(p_skeleton, angular_delta_limit);
+ distance_to_target_sq = p_setting->chain[p_setting->chain.size() - 1].distance_squared_to(p_destination);
+ iteration_count++;
+ }
+
+ // Apply the virtual bone rest/poses to the actual bones.
+ for (uint32_t i = 0; i < p_setting->solver_info_list.size(); i++) {
+ IKModifier3DSolverInfo *solver_info = p_setting->solver_info_list[i];
+ if (!solver_info || Math::is_zero_approx(solver_info->length)) {
+ continue;
+ }
+ p_skeleton->set_bone_pose_rotation(p_setting->joints[i].bone, solver_info->current_lpose);
+ }
+
+ p_setting->simulated = true;
+}
+
+void IterateIK3D::_solve_iteration(double p_delta, Skeleton3D *p_skeleton, IterateIK3DSetting *p_setting, const Vector3 &p_destination) {
+ //
+}
+
+void IterateIK3D::_update_joint_limitation(int p_index, int p_joint) {
+ ERR_FAIL_INDEX(p_index, (int)iterate_settings.size());
+ iterate_settings[p_index]->simulated = false;
+ LocalVector &joint_settings = iterate_settings[p_index]->joint_settings;
+ ERR_FAIL_INDEX(p_joint, (int)joint_settings.size()); // p_joint is unused directly, but need to identify bound index.
+#ifdef TOOLS_ENABLED
+ update_gizmos();
+#endif // TOOLS_ENABLED
+}
+
+void IterateIK3D::_bind_joint_limitation(int p_index, int p_joint) {
+ ERR_FAIL_INDEX(p_index, (int)iterate_settings.size());
+ LocalVector &joint_settings = iterate_settings[p_index]->joint_settings;
+ ERR_FAIL_INDEX(p_joint, (int)joint_settings.size());
+ if (joint_settings[p_joint]->limitation.is_valid()) {
+ joint_settings[p_joint]->limitation->connect_changed(callable_mp(this, &IterateIK3D::_update_joint_limitation).bind(p_index, p_joint));
+ }
+}
+
+void IterateIK3D::_unbind_joint_limitation(int p_index, int p_joint) {
+ ERR_FAIL_INDEX(p_index, (int)iterate_settings.size());
+ LocalVector &joint_settings = iterate_settings[p_index]->joint_settings;
+ ERR_FAIL_INDEX(p_joint, (int)joint_settings.size());
+ if (joint_settings[p_joint]->limitation.is_valid()) {
+ joint_settings[p_joint]->limitation->disconnect_changed(callable_mp(this, &IterateIK3D::_update_joint_limitation).bind(p_index, p_joint));
+ }
+}
+
+void IterateIK3D::_bind_joint_limitations(int p_index) {
+ for (uint32_t i = 0; i < iterate_settings[p_index]->joints.size(); i++) {
+ if (iterate_settings[p_index]->joint_settings[i]->limitation.is_valid()) {
+ iterate_settings[p_index]->joint_settings[i]->limitation->connect_changed(callable_mp(this, &IterateIK3D::_update_joint_limitation).bind(p_index, i));
+ }
+ }
+}
+
+void IterateIK3D::_unbind_joint_limitations(int p_index) {
+ for (uint32_t i = 0; i < iterate_settings[p_index]->joint_settings.size(); i++) {
+ if (iterate_settings[p_index]->joint_settings[i]->limitation.is_valid()) {
+ iterate_settings[p_index]->joint_settings[i]->limitation->disconnect_changed(callable_mp(this, &IterateIK3D::_update_joint_limitation).bind(p_index, i));
+ }
+ }
+}
+
+IterateIK3D::~IterateIK3D() {
+ for (uint32_t i = 0; i < iterate_settings.size(); i++) {
+ _unbind_joint_limitations(i);
+ }
+ clear_settings();
+}
diff --git a/scene/3d/iterate_ik_3d.h b/scene/3d/iterate_ik_3d.h
new file mode 100644
index 00000000000..0bb917da45c
--- /dev/null
+++ b/scene/3d/iterate_ik_3d.h
@@ -0,0 +1,315 @@
+/**************************************************************************/
+/* iterate_ik_3d.h */
+/**************************************************************************/
+/* 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. */
+/**************************************************************************/
+
+#pragma once
+
+#include "scene/3d/chain_ik_3d.h"
+
+#include "scene/resources/3d/joint_limitation_3d.h"
+
+class IterateIK3D : public ChainIK3D {
+ GDCLASS(IterateIK3D, ChainIK3D);
+
+public:
+ struct IterateIK3DJointSetting {
+ // To limit rotation.
+ RotationAxis rotation_axis = ROTATION_AXIS_ALL;
+ Vector3 rotation_axis_vector = Vector3(1, 0, 0);
+ Ref limitation;
+ SecondaryDirection limitation_right_axis = SECONDARY_DIRECTION_NONE;
+ Vector3 limitation_right_axis_vector = Vector3(1, 0, 0);
+ Quaternion limitation_rotation_offset;
+
+ // Rotation axis.
+ Vector3 get_rotation_axis_vector() const {
+ Vector3 ret;
+ switch (rotation_axis) {
+ case ROTATION_AXIS_X:
+ ret = Vector3(1, 0, 0);
+ break;
+ case ROTATION_AXIS_Y:
+ ret = Vector3(0, 1, 0);
+ break;
+ case ROTATION_AXIS_Z:
+ ret = Vector3(0, 0, 1);
+ break;
+ case ROTATION_AXIS_ALL:
+ ret = Vector3(0, 0, 0);
+ break;
+ case ROTATION_AXIS_CUSTOM:
+ ret = rotation_axis_vector;
+ break;
+ }
+ return ret;
+ }
+
+ Vector3 get_limitation_right_axis_vector() const {
+ Vector3 ret;
+ switch (limitation_right_axis) {
+ case SECONDARY_DIRECTION_NONE:
+ ret = Vector3(0, 0, 0);
+ break;
+ case SECONDARY_DIRECTION_PLUS_X:
+ ret = Vector3(1, 0, 0);
+ break;
+ case SECONDARY_DIRECTION_MINUS_X:
+ ret = Vector3(-1, 0, 0);
+ break;
+ case SECONDARY_DIRECTION_PLUS_Y:
+ ret = Vector3(0, 1, 0);
+ break;
+ case SECONDARY_DIRECTION_MINUS_Y:
+ ret = Vector3(0, -1, 0);
+ break;
+ case SECONDARY_DIRECTION_PLUS_Z:
+ ret = Vector3(0, 0, 1);
+ break;
+ case SECONDARY_DIRECTION_MINUS_Z:
+ ret = Vector3(0, 0, -1);
+ break;
+ case SECONDARY_DIRECTION_CUSTOM:
+ ret = limitation_right_axis_vector;
+ break;
+ }
+ return ret;
+ }
+
+ Quaternion get_limitation_space(const Vector3 &p_local_forward) const {
+ if (limitation.is_null()) {
+ return Quaternion();
+ }
+ return limitation->make_space(p_local_forward, get_limitation_right_axis_vector(), limitation_rotation_offset);
+ }
+
+ // Get rotation around normal vector (normal vector is rotation axis).
+ Vector3 get_projected_rotation(const Quaternion &p_offset, const Vector3 &p_vector) const {
+ ERR_FAIL_COND_V(rotation_axis == ROTATION_AXIS_ALL, p_vector);
+ const double ALMOST_ONE = 1.0 - CMP_EPSILON;
+ Vector3 axis = get_rotation_axis_vector().normalized();
+ Vector3 local_vector = p_offset.xform_inv(p_vector);
+ double length = local_vector.length();
+ Vector3 projected = snap_vector_to_plane(axis, local_vector.normalized());
+ if (!Math::is_zero_approx(length)) {
+ projected = projected.normalized() * length;
+ }
+ if (Math::abs(local_vector.normalized().dot(axis)) > ALMOST_ONE) {
+ return p_vector;
+ }
+ return p_offset.xform(projected);
+ }
+
+ // Get limited rotation from forward axis in local rest space.
+ Vector3 get_limited_rotation(const Quaternion &p_offset, const Vector3 &p_vector, const Vector3 &p_forward) const {
+ ERR_FAIL_COND_V(limitation.is_null(), p_vector);
+ Vector3 local_vector = p_offset.xform_inv(p_vector);
+ float length = local_vector.length();
+ if (Math::is_zero_approx(length)) {
+ return p_vector;
+ }
+ Vector3 limited = limitation->solve(p_forward, get_limitation_right_axis_vector(), limitation_rotation_offset, local_vector.normalized()) * length;
+ return p_offset.xform(limited);
+ }
+
+ ~IterateIK3DJointSetting() {
+ limitation.unref();
+ }
+ };
+
+ struct IterateIK3DSetting : public ChainIK3DSetting {
+ NodePath target_node;
+
+ LocalVector joint_settings;
+
+ bool simulated = false;
+
+ bool is_penetrated(const Vector3 &p_destination) {
+ bool ret = false;
+ Vector3 chain_dir = (chain[chain.size() - 1] - chain[0]).normalized();
+ bool is_straight = true;
+ for (uint32_t i = 1; i < chain.size() - 1; i++) {
+ Vector3 dir = (chain[i] - chain[0]).normalized();
+ if (!dir.is_equal_approx(chain_dir)) {
+ is_straight = false;
+ break;
+ }
+ }
+ if (is_straight) {
+ Vector3 to_target = (p_destination - chain[0]);
+ double proj = to_target.dot(chain_dir);
+ double total_length = 0;
+ for (uint32_t i = 0; i < solver_info_list.size(); i++) {
+ if (solver_info_list[i]) {
+ total_length += solver_info_list[i]->length;
+ }
+ }
+ ret = proj >= 0 && proj <= total_length && (to_target.normalized().is_equal_approx(chain_dir));
+ }
+ return ret;
+ }
+
+ // Make rotation as bone pose from chain coordinates.
+ // p_extra is delta angle limitation.
+ void cache_current_joint_rotations(Skeleton3D *p_skeleton, double p_angular_delta_limit = Math::PI) {
+ Transform3D parent_gpose_tr;
+ int parent = p_skeleton->get_bone_parent(root_bone.bone);
+ if (parent >= 0) {
+ parent_gpose_tr = p_skeleton->get_bone_global_pose(parent);
+ }
+ Quaternion parent_gpose = parent_gpose_tr.basis.get_rotation_quaternion();
+
+ for (uint32_t i = 0; i < joints.size(); i++) {
+ int HEAD = i;
+ IKModifier3DSolverInfo *solver_info = solver_info_list[HEAD];
+ if (!solver_info) {
+ continue;
+ }
+ solver_info->current_lrest = p_skeleton->get_bone_pose(joints[HEAD].bone).basis.get_rotation_quaternion();
+ solver_info->current_grest = parent_gpose * solver_info->current_lrest;
+ solver_info->current_grest.normalize();
+ Vector3 from = solver_info->forward_vector;
+ Vector3 to = solver_info->current_grest.xform_inv(solver_info->current_vector).normalized();
+ Quaternion prev = solver_info->current_lpose;
+ if (joint_settings[HEAD]->rotation_axis == ROTATION_AXIS_ALL) {
+ solver_info->current_lpose = solver_info->current_lrest * get_swing(Quaternion(from, to), from);
+ } else {
+ // To stabilize rotation path especially nearely 180deg.
+ solver_info->current_lpose = solver_info->current_lrest * get_from_to_rotation_by_axis(from, to, joint_settings[HEAD]->get_rotation_axis_vector().normalized());
+ }
+ double diff = prev.angle_to(solver_info->current_lpose);
+ if (!Math::is_zero_approx(diff)) {
+ solver_info->current_lpose = prev.slerp(solver_info->current_lpose, MIN(1.0, p_angular_delta_limit / diff));
+ }
+ solver_info->current_gpose = parent_gpose * solver_info->current_lpose;
+ solver_info->current_gpose.normalize();
+ parent_gpose = solver_info->current_gpose;
+ }
+
+ // Apply back angular_delta_limit to chain coordinates.
+ if (chain.is_empty()) {
+ return;
+ }
+ chain[0] = p_skeleton->get_bone_global_pose(root_bone.bone).origin;
+ for (uint32_t i = 0; i < solver_info_list.size(); i++) {
+ int HEAD = i;
+ int TAIL = i + 1;
+ IKModifier3DSolverInfo *solver_info = solver_info_list[HEAD];
+ if (!solver_info) {
+ continue;
+ }
+ chain[TAIL] = chain[HEAD] + solver_info->current_gpose.xform(solver_info->forward_vector) * solver_info->length;
+ }
+ cache_current_vectors(p_skeleton);
+ }
+
+ ~IterateIK3DSetting() {
+ for (uint32_t i = 0; i < joint_settings.size(); i++) {
+ if (joint_settings[i]) {
+ memdelete(joint_settings[i]);
+ joint_settings[i] = nullptr;
+ }
+ }
+ joint_settings.clear();
+ }
+ };
+
+protected:
+ LocalVector iterate_settings; // For caching.
+
+ int max_iterations = 4;
+ double min_distance = 0.001; // If distance between end joint and target is less than min_distance, finish iteration.
+ double min_distance_squared = min_distance * min_distance; // For cache.
+ double angular_delta_limit = Math::deg_to_rad(2.0); // If the delta is too large, the results before and after iterating can change significantly, and divergence of calculations can easily occur.
+
+ bool _get(const StringName &p_path, Variant &r_ret) const;
+ bool _set(const StringName &p_path, const Variant &p_value);
+ void _get_property_list(List *p_list) const;
+ void _validate_dynamic_prop(PropertyInfo &p_property) const;
+
+ static void _bind_methods();
+
+ virtual void _validate_axis(Skeleton3D *p_skeleton, int p_index, int p_joint) const override;
+ virtual void _init_joints(Skeleton3D *p_skeleton, int p_index) override;
+ virtual void _make_simulation_dirty(int p_index) override;
+
+ virtual void _process_ik(Skeleton3D *p_skeleton, double p_delta) override;
+ void _process_joints(double p_delta, Skeleton3D *p_skeleton, IterateIK3DSetting *p_setting, const Vector3 &p_target_destination);
+ virtual void _solve_iteration(double p_delta, Skeleton3D *p_skeleton, IterateIK3DSetting *p_setting, const Vector3 &p_destination);
+
+ virtual void _set_joint_count(int p_index, int p_count) override;
+
+ void _update_joint_limitation(int p_index, int p_joint);
+ void _bind_joint_limitation(int p_index, int p_joint);
+ void _unbind_joint_limitation(int p_index, int p_joint);
+ void _bind_joint_limitations(int p_index);
+ void _unbind_joint_limitations(int p_index);
+
+public:
+ virtual PackedStringArray get_configuration_warnings() const override;
+ virtual void set_setting_count(int p_count) override {
+ _set_setting_count(p_count);
+ iterate_settings = _cast_settings();
+ chain_settings = _cast_settings(); // Don't forget to sync super class settings.
+ }
+ virtual void clear_settings() override {
+ _set_setting_count(0);
+ iterate_settings.clear();
+ chain_settings.clear(); // Don't forget to sync super class settings.
+ }
+
+ void set_max_iterations(int p_max_iterations);
+ int get_max_iterations() const;
+ void set_min_distance(double p_min_distance);
+ double get_min_distance() const;
+ void set_angular_delta_limit(double p_angular_delta_limit);
+ double get_angular_delta_limit() const;
+
+ // Setting.
+ void set_target_node(int p_index, const NodePath &p_target_node);
+ NodePath get_target_node(int p_index) const;
+
+ // Individual joints.
+ void set_joint_rotation_axis(int p_index, int p_joint, RotationAxis p_axis);
+ RotationAxis get_joint_rotation_axis(int p_index, int p_joint) const;
+ void set_joint_rotation_axis_vector(int p_index, int p_joint, const Vector3 &p_vector);
+ Vector3 get_joint_rotation_axis_vector(int p_index, int p_joint) const;
+ void set_joint_limitation(int p_index, int p_joint, const Ref &p_limitation);
+ Ref get_joint_limitation(int p_index, int p_joint) const;
+ void set_joint_limitation_right_axis(int p_index, int p_joint, SecondaryDirection p_direction);
+ SecondaryDirection get_joint_limitation_right_axis(int p_index, int p_joint) const;
+ void set_joint_limitation_right_axis_vector(int p_index, int p_joint, const Vector3 &p_vector);
+ Vector3 get_joint_limitation_right_axis_vector(int p_index, int p_joint) const;
+ void set_joint_limitation_rotation_offset(int p_index, int p_joint, const Quaternion &p_offset);
+ Quaternion get_joint_limitation_rotation_offset(int p_index, int p_joint) const;
+
+ // Helper.
+ Quaternion get_joint_limitation_space(int p_index, int p_joint, const Vector3 &p_forward) const;
+
+ ~IterateIK3D();
+};
diff --git a/scene/3d/jacobian_ik_3d.cpp b/scene/3d/jacobian_ik_3d.cpp
new file mode 100644
index 00000000000..a6dbdfad8c0
--- /dev/null
+++ b/scene/3d/jacobian_ik_3d.cpp
@@ -0,0 +1,72 @@
+/**************************************************************************/
+/* jacobian_ik_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 "jacobian_ik_3d.h"
+
+void JacobianIK3D::_solve_iteration(double p_delta, Skeleton3D *p_skeleton, IterateIK3DSetting *p_setting, const Vector3 &p_destination) {
+ int joint_size = (int)p_setting->joints.size();
+ int chain_size = (int)p_setting->chain.size();
+
+ // Forwards.
+ for (int i = 0; i < joint_size; i++) {
+ IKModifier3DSolverInfo *solver_info = p_setting->solver_info_list[i];
+ if (!solver_info || Math::is_zero_approx(solver_info->length)) {
+ continue;
+ }
+
+ int HEAD = i;
+ int TAIL = i + 1;
+
+ Vector3 current_head = p_setting->chain[HEAD];
+ Vector3 current_effector = p_setting->chain[chain_size - 1];
+ Vector3 head_to_effector = current_effector - current_head;
+ Vector3 effector_to_destination = p_destination - current_effector;
+ Vector3 axis = head_to_effector.cross(effector_to_destination);
+
+ if (Math::is_zero_approx(axis.length_squared())) {
+ continue;
+ }
+
+ Quaternion to_rot = Quaternion(axis.normalized(), axis.length() / MAX(CMP_EPSILON, head_to_effector.length()));
+
+ for (int j = TAIL; j < chain_size; j++) {
+ Vector3 to_tail = p_setting->chain[j] - current_head;
+ p_setting->update_chain_coordinate_fw(p_skeleton, j, current_head + to_rot.xform(to_tail));
+
+ int k = j - 1;
+ if (p_setting->joint_settings[k]->rotation_axis != ROTATION_AXIS_ALL) {
+ p_setting->update_chain_coordinate_fw(p_skeleton, j, p_setting->chain[k] + p_setting->joint_settings[k]->get_projected_rotation(solver_info->current_grest, p_setting->chain[j] - p_setting->chain[k]));
+ }
+ if (p_setting->joint_settings[k]->limitation.is_valid()) {
+ p_setting->update_chain_coordinate_fw(p_skeleton, j, p_setting->chain[k] + p_setting->joint_settings[k]->get_limited_rotation(solver_info->current_grest, p_setting->chain[j] - p_setting->chain[k], solver_info->forward_vector));
+ }
+ }
+ }
+}
diff --git a/scene/3d/jacobian_ik_3d.h b/scene/3d/jacobian_ik_3d.h
new file mode 100644
index 00000000000..6fd441750db
--- /dev/null
+++ b/scene/3d/jacobian_ik_3d.h
@@ -0,0 +1,40 @@
+/**************************************************************************/
+/* jacobian_ik_3d.h */
+/**************************************************************************/
+/* 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. */
+/**************************************************************************/
+
+#pragma once
+
+#include "scene/3d/iterate_ik_3d.h"
+
+class JacobianIK3D : public IterateIK3D {
+ GDCLASS(JacobianIK3D, IterateIK3D);
+
+protected:
+ virtual void _solve_iteration(double p_delta, Skeleton3D *p_skeleton, IterateIK3DSetting *p_setting, const Vector3 &p_destination) override;
+};
diff --git a/scene/3d/look_at_modifier_3d.cpp b/scene/3d/look_at_modifier_3d.cpp
index 401f9ea399e..3dbad6aa896 100644
--- a/scene/3d/look_at_modifier_3d.cpp
+++ b/scene/3d/look_at_modifier_3d.cpp
@@ -34,7 +34,7 @@ void LookAtModifier3D::_validate_property(PropertyInfo &p_property) const {
if (Engine::get_singleton()->is_editor_hint() && (p_property.name == "bone_name" || p_property.name == "origin_bone_name")) {
Skeleton3D *skeleton = get_skeleton();
if (skeleton) {
- p_property.hint = PROPERTY_HINT_ENUM;
+ p_property.hint = PROPERTY_HINT_ENUM_SUGGESTION;
p_property.hint_string = skeleton->get_concatenated_bone_names();
} else {
p_property.hint = PROPERTY_HINT_NONE;
@@ -457,7 +457,7 @@ void LookAtModifier3D::_bind_methods() {
ADD_PROPERTY(PropertyInfo(Variant::STRING, "bone_name", PROPERTY_HINT_ENUM_SUGGESTION, ""), "set_bone_name", "get_bone_name");
ADD_PROPERTY(PropertyInfo(Variant::INT, "bone", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_NO_EDITOR), "set_bone", "get_bone");
- ADD_PROPERTY(PropertyInfo(Variant::INT, "forward_axis", PROPERTY_HINT_ENUM, "+X,-X,+Y,-Y,+Z,-Z"), "set_forward_axis", "get_forward_axis");
+ ADD_PROPERTY(PropertyInfo(Variant::INT, "forward_axis", PROPERTY_HINT_ENUM, SkeletonModifier3D::get_hint_bone_axis()), "set_forward_axis", "get_forward_axis");
ADD_PROPERTY(PropertyInfo(Variant::INT, "primary_rotation_axis", PROPERTY_HINT_ENUM, "X,Y,Z"), "set_primary_rotation_axis", "get_primary_rotation_axis");
ADD_PROPERTY(PropertyInfo(Variant::BOOL, "use_secondary_rotation"), "set_use_secondary_rotation", "is_using_secondary_rotation");
@@ -500,10 +500,6 @@ void LookAtModifier3D::_bind_methods() {
}
void LookAtModifier3D::_process_modification(double p_delta) {
- if (!is_inside_tree()) {
- return;
- }
-
Skeleton3D *skeleton = get_skeleton();
if (!skeleton || bone < 0 || bone >= skeleton->get_bone_count()) {
return;
diff --git a/scene/3d/modifier_bone_target_3d.cpp b/scene/3d/modifier_bone_target_3d.cpp
index 9be977cf82a..6539a4afacd 100644
--- a/scene/3d/modifier_bone_target_3d.cpp
+++ b/scene/3d/modifier_bone_target_3d.cpp
@@ -78,7 +78,7 @@ void ModifierBoneTarget3D::_validate_property(PropertyInfo &p_property) const {
if (p_property.name == "bone_name") {
Skeleton3D *skeleton = get_skeleton();
if (skeleton) {
- p_property.hint = PROPERTY_HINT_ENUM;
+ p_property.hint = PROPERTY_HINT_ENUM_SUGGESTION;
p_property.hint_string = skeleton->get_concatenated_bone_names();
} else {
p_property.hint = PROPERTY_HINT_NONE;
@@ -98,10 +98,6 @@ void ModifierBoneTarget3D::_bind_methods() {
}
void ModifierBoneTarget3D::_process_modification(double p_delta) {
- if (!is_inside_tree()) {
- return;
- }
-
Skeleton3D *skeleton = get_skeleton();
if (!skeleton || bone < 0 || bone >= skeleton->get_bone_count()) {
return;
diff --git a/scene/3d/path_3d.h b/scene/3d/path_3d.h
index 5dc69d4454e..dfb4705997c 100644
--- a/scene/3d/path_3d.h
+++ b/scene/3d/path_3d.h
@@ -63,9 +63,6 @@ public:
const Color &get_debug_custom_color() const;
void set_debug_custom_color(const Color &p_color);
- bool get_debug_show() const;
- void set_debug_show(bool p_show);
-
Ref get_debug_material();
Path3D();
diff --git a/scene/3d/skeleton_3d.cpp b/scene/3d/skeleton_3d.cpp
index 332fb03e9eb..a66655ebd3a 100644
--- a/scene/3d/skeleton_3d.cpp
+++ b/scene/3d/skeleton_3d.cpp
@@ -942,7 +942,7 @@ void Skeleton3D::_update_deferred(UpdateFlag p_update_flag) {
_notification(NOTIFICATION_UPDATE_SKELETON);
return;
}
-#endif //TOOLS_ENABLED
+#endif // TOOLS_ENABLED
if (update_flags == UPDATE_FLAG_NONE && !updating) {
notify_deferred_thread_group(NOTIFICATION_UPDATE_SKELETON); // It must never be called more than once in a single frame.
}
@@ -1177,7 +1177,7 @@ void Skeleton3D::_process_modifiers() {
if (saving && !mod->is_processed_on_saving()) {
continue;
}
-#endif //TOOLS_ENABLED
+#endif // TOOLS_ENABLED
real_t influence = mod->get_influence();
if (influence < 1.0) {
LocalVector old_poses;
diff --git a/scene/3d/skeleton_3d.h b/scene/3d/skeleton_3d.h
index 8626be97f20..6fc6f375847 100644
--- a/scene/3d/skeleton_3d.h
+++ b/scene/3d/skeleton_3d.h
@@ -67,7 +67,7 @@ class Skeleton3D : public Node3D {
#ifdef TOOLS_ENABLED
bool saving = false;
-#endif //TOOLS_ENABLED
+#endif // TOOLS_ENABLED
#if !defined(DISABLE_DEPRECATED) && !defined(PHYSICS_3D_DISABLED)
bool animate_physical_bones = true;
diff --git a/scene/3d/skeleton_modifier_3d.cpp b/scene/3d/skeleton_modifier_3d.cpp
index d4c157734ad..3c34c875f17 100644
--- a/scene/3d/skeleton_modifier_3d.cpp
+++ b/scene/3d/skeleton_modifier_3d.cpp
@@ -117,7 +117,7 @@ real_t SkeletonModifier3D::get_influence() const {
}
void SkeletonModifier3D::process_modification(double p_delta) {
- if (!active) {
+ if (!is_inside_tree() || !active) {
return;
}
_process_modification(p_delta);
@@ -175,6 +175,29 @@ void SkeletonModifier3D::_bind_methods() {
BIND_ENUM_CONSTANT(BONE_AXIS_MINUS_Y);
BIND_ENUM_CONSTANT(BONE_AXIS_PLUS_Z);
BIND_ENUM_CONSTANT(BONE_AXIS_MINUS_Z);
+
+ BIND_ENUM_CONSTANT(BONE_DIRECTION_PLUS_X);
+ BIND_ENUM_CONSTANT(BONE_DIRECTION_MINUS_X);
+ BIND_ENUM_CONSTANT(BONE_DIRECTION_PLUS_Y);
+ BIND_ENUM_CONSTANT(BONE_DIRECTION_MINUS_Y);
+ BIND_ENUM_CONSTANT(BONE_DIRECTION_PLUS_Z);
+ BIND_ENUM_CONSTANT(BONE_DIRECTION_MINUS_Z);
+ BIND_ENUM_CONSTANT(BONE_DIRECTION_FROM_PARENT);
+
+ BIND_ENUM_CONSTANT(SECONDARY_DIRECTION_NONE);
+ BIND_ENUM_CONSTANT(SECONDARY_DIRECTION_PLUS_X);
+ BIND_ENUM_CONSTANT(SECONDARY_DIRECTION_MINUS_X);
+ BIND_ENUM_CONSTANT(SECONDARY_DIRECTION_PLUS_Y);
+ BIND_ENUM_CONSTANT(SECONDARY_DIRECTION_MINUS_Y);
+ BIND_ENUM_CONSTANT(SECONDARY_DIRECTION_PLUS_Z);
+ BIND_ENUM_CONSTANT(SECONDARY_DIRECTION_MINUS_Z);
+ BIND_ENUM_CONSTANT(SECONDARY_DIRECTION_CUSTOM);
+
+ BIND_ENUM_CONSTANT(ROTATION_AXIS_X);
+ BIND_ENUM_CONSTANT(ROTATION_AXIS_Y);
+ BIND_ENUM_CONSTANT(ROTATION_AXIS_Z);
+ BIND_ENUM_CONSTANT(ROTATION_AXIS_ALL);
+ BIND_ENUM_CONSTANT(ROTATION_AXIS_CUSTOM);
}
Vector3 SkeletonModifier3D::get_vector_from_bone_axis(BoneAxis p_axis) {
@@ -246,7 +269,7 @@ Quaternion SkeletonModifier3D::get_local_pose_rotation(Skeleton3D *p_skeleton, i
if (parent < 0) {
return p_global_pose_rotation;
}
- return p_skeleton->get_bone_global_pose(parent).basis.orthonormalized().inverse() * p_global_pose_rotation;
+ return (p_skeleton->get_bone_global_pose(parent).basis.get_rotation_quaternion().inverse() * p_global_pose_rotation).normalized();
}
Quaternion SkeletonModifier3D::get_from_to_rotation(const Vector3 &p_from, const Vector3 &p_to, const Quaternion &p_prev_rot) {
@@ -264,6 +287,47 @@ Quaternion SkeletonModifier3D::get_from_to_rotation(const Vector3 &p_from, const
return Quaternion(axis.normalized(), angle);
}
+Quaternion SkeletonModifier3D::get_from_to_rotation_by_axis(const Vector3 &p_from, const Vector3 &p_to, const Vector3 &p_axis) {
+ const double ALMOST_ONE = 1.0 - CMP_EPSILON;
+ double dot = p_from.dot(p_to);
+ if (dot > ALMOST_ONE) {
+ return Quaternion();
+ }
+ if (dot < -ALMOST_ONE) {
+ return Quaternion(p_axis, Math::PI);
+ }
+ double angle = p_from.angle_to(p_to);
+ Vector3 cross = p_from.cross(p_to);
+ if (std::signbit(cross.dot(p_axis))) {
+ angle = -angle;
+ }
+ return Quaternion(p_axis, angle);
+}
+
+Quaternion SkeletonModifier3D::get_swing(const Quaternion &p_rotation, const Vector3 &p_axis) {
+ if (p_axis.is_zero_approx()) {
+ return p_rotation;
+ }
+ Quaternion rot = p_rotation;
+ if (!rot.is_normalized()) {
+ rot.normalize();
+ }
+ Vector3 axis = p_axis.normalized();
+ const Vector3 v(rot.x, rot.y, rot.z);
+ const real_t proj_len = v.dot(axis);
+ const Vector3 twist_vec = axis * proj_len;
+ Quaternion twist(twist_vec.x, twist_vec.y, twist_vec.z, rot.w);
+ if (!twist.is_normalized()) {
+ if (Math::is_zero_approx(twist.length_squared())) {
+ return rot;
+ }
+ twist.normalize();
+ }
+ Quaternion swing = rot * twist.inverse();
+ swing.normalize();
+ return swing;
+}
+
Vector3 SkeletonModifier3D::snap_vector_to_plane(const Vector3 &p_plane_normal, const Vector3 &p_vector) {
if (Math::is_zero_approx(p_plane_normal.length_squared())) {
return p_vector;
@@ -274,5 +338,57 @@ Vector3 SkeletonModifier3D::snap_vector_to_plane(const Vector3 &p_plane_normal,
return normalized_vec.slide(normal) * length;
}
+double SkeletonModifier3D::symmetrize_angle(double p_angle) {
+ double angle = Math::fposmod(p_angle, Math::TAU);
+ return angle > Math::PI ? angle - Math::TAU : angle;
+}
+
+double SkeletonModifier3D::get_roll_angle(const Quaternion &p_rotation, const Vector3 &p_roll_axis) {
+ // Ensure roll axis is normalized.
+ Vector3 roll_axis = p_roll_axis.normalized();
+
+ // Project the quaternion rotation onto the roll axis.
+ // This gives us the component of rotation around that axis.
+ double dot = p_rotation.x * roll_axis.x +
+ p_rotation.y * roll_axis.y +
+ p_rotation.z * roll_axis.z;
+
+ // Create a quaternion representing just the roll component.
+ Quaternion roll_component;
+ roll_component.x = roll_axis.x * dot;
+ roll_component.y = roll_axis.y * dot;
+ roll_component.z = roll_axis.z * dot;
+ roll_component.w = p_rotation.w;
+
+ // Normalize this component.
+ double length = roll_component.length();
+ if (length > CMP_EPSILON) {
+ roll_component = roll_component / length;
+ } else {
+ return 0.0;
+ }
+
+ // Extract the angle.
+ double angle = 2.0 * Math::acos(CLAMP(roll_component.w, -1.0, 1.0));
+
+ // Determine the sign.
+ double direction = (roll_component.x * roll_axis.x + roll_component.y * roll_axis.y + roll_component.z * roll_axis.z > 0) ? 1.0 : -1.0;
+
+ return symmetrize_angle(angle * direction);
+}
+
+Vector3 SkeletonModifier3D::get_projected_normal(const Vector3 &p_a, const Vector3 &p_b, const Vector3 &p_point) {
+ // Get nearest normal vector to p_point from the infinite line p_a to p_b.
+ const Vector3 dir = p_b - p_a;
+ const real_t denom = dir.length_squared();
+ if (Math::is_zero_approx(denom)) {
+ return Vector3();
+ }
+ const Vector3 w = p_point - p_a;
+ const real_t t = w.dot(dir) / denom;
+ const Vector3 h = p_a + dir * t;
+ return (p_point - h).normalized();
+}
+
SkeletonModifier3D::SkeletonModifier3D() {
}
diff --git a/scene/3d/skeleton_modifier_3d.h b/scene/3d/skeleton_modifier_3d.h
index 8a3913367a0..25e46953377 100644
--- a/scene/3d/skeleton_modifier_3d.h
+++ b/scene/3d/skeleton_modifier_3d.h
@@ -40,6 +40,7 @@ class SkeletonModifier3D : public Node3D {
void rebind();
public:
+ // For the case to indicate bone axis on basis without custom vector.
enum BoneAxis {
BONE_AXIS_PLUS_X,
BONE_AXIS_MINUS_X,
@@ -48,6 +49,42 @@ public:
BONE_AXIS_PLUS_Z,
BONE_AXIS_MINUS_Z,
};
+ static String get_hint_bone_axis() { return "+X,-X,+Y,-Y,+Z,-Z"; }
+
+ // For the case to indicate Head-Tail of the bone.
+ enum BoneDirection {
+ BONE_DIRECTION_PLUS_X,
+ BONE_DIRECTION_MINUS_X,
+ BONE_DIRECTION_PLUS_Y,
+ BONE_DIRECTION_MINUS_Y,
+ BONE_DIRECTION_PLUS_Z,
+ BONE_DIRECTION_MINUS_Z,
+ BONE_DIRECTION_FROM_PARENT,
+ };
+ static String get_hint_bone_direction() { return "+X,-X,+Y,-Y,+Z,-Z,FromParent"; }
+
+ // For the case to define secondary axis of the bone local space.
+ enum SecondaryDirection {
+ SECONDARY_DIRECTION_NONE,
+ SECONDARY_DIRECTION_PLUS_X,
+ SECONDARY_DIRECTION_MINUS_X,
+ SECONDARY_DIRECTION_PLUS_Y,
+ SECONDARY_DIRECTION_MINUS_Y,
+ SECONDARY_DIRECTION_PLUS_Z,
+ SECONDARY_DIRECTION_MINUS_Z,
+ SECONDARY_DIRECTION_CUSTOM,
+ };
+ static String get_hint_secondary_direction() { return "None,+X,-X,+Y,-Y,+Z,-Z,Custom"; }
+
+ // For the case to define rotation direction without identification plus/minus.
+ enum RotationAxis {
+ ROTATION_AXIS_X,
+ ROTATION_AXIS_Y,
+ ROTATION_AXIS_Z,
+ ROTATION_AXIS_ALL,
+ ROTATION_AXIS_CUSTOM,
+ };
+ static String get_hint_rotation_axis() { return "X,Y,Z,All,Custom"; }
protected:
bool active = true;
@@ -96,10 +133,16 @@ public:
static Vector3 get_vector_from_axis(Vector3::Axis p_axis);
static Vector3::Axis get_axis_from_bone_axis(BoneAxis p_axis);
+ // 3D math.
static Vector3 limit_length(const Vector3 &p_origin, const Vector3 &p_destination, float p_length);
static Quaternion get_local_pose_rotation(Skeleton3D *p_skeleton, int p_bone, const Quaternion &p_global_pose_rotation);
static Quaternion get_from_to_rotation(const Vector3 &p_from, const Vector3 &p_to, const Quaternion &p_prev_rot);
+ static Quaternion get_from_to_rotation_by_axis(const Vector3 &p_from, const Vector3 &p_to, const Vector3 &p_axis);
+ static Quaternion get_swing(const Quaternion &p_rotation, const Vector3 &p_axis);
static Vector3 snap_vector_to_plane(const Vector3 &p_plane_normal, const Vector3 &p_vector);
+ static double symmetrize_angle(double p_angle);
+ static double get_roll_angle(const Quaternion &p_rotation, const Vector3 &p_roll_axis);
+ static Vector3 get_projected_normal(const Vector3 &p_a, const Vector3 &p_b, const Vector3 &p_point);
#ifdef TOOLS_ENABLED
virtual bool is_processed_on_saving() const { return false; }
@@ -109,3 +152,6 @@ public:
};
VARIANT_ENUM_CAST(SkeletonModifier3D::BoneAxis);
+VARIANT_ENUM_CAST(SkeletonModifier3D::BoneDirection);
+VARIANT_ENUM_CAST(SkeletonModifier3D::SecondaryDirection);
+VARIANT_ENUM_CAST(SkeletonModifier3D::RotationAxis);
diff --git a/scene/3d/spline_ik_3d.cpp b/scene/3d/spline_ik_3d.cpp
new file mode 100644
index 00000000000..9e02620f09e
--- /dev/null
+++ b/scene/3d/spline_ik_3d.cpp
@@ -0,0 +1,435 @@
+/**************************************************************************/
+/* spline_ik_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 "spline_ik_3d.h"
+
+bool SplineIK3D::_set(const StringName &p_path, const Variant &p_value) {
+ String path = p_path;
+
+ if (path.begins_with("settings/")) {
+ int which = path.get_slicec('/', 1).to_int();
+ String what = path.get_slicec('/', 2);
+ ERR_FAIL_INDEX_V(which, (int)settings.size(), false);
+
+ if (what == "path_3d") {
+ set_path_3d(which, p_value);
+ } else if (what == "tilt_enabled") {
+ set_tilt_enabled(which, p_value);
+ } else if (what == "tilt_fade_in") {
+ set_tilt_fade_in(which, p_value);
+ } else if (what == "tilt_fade_out") {
+ set_tilt_fade_out(which, p_value);
+ } else {
+ return false;
+ }
+ }
+ return true;
+}
+
+bool SplineIK3D::_get(const StringName &p_path, Variant &r_ret) const {
+ String path = p_path;
+
+ if (path.begins_with("settings/")) {
+ int which = path.get_slicec('/', 1).to_int();
+ String what = path.get_slicec('/', 2);
+ ERR_FAIL_INDEX_V(which, (int)settings.size(), false);
+
+ if (what == "path_3d") {
+ r_ret = get_path_3d(which);
+ } else if (what == "tilt_enabled") {
+ r_ret = is_tilt_enabled(which);
+ } else if (what == "tilt_fade_in") {
+ r_ret = get_tilt_fade_in(which);
+ } else if (what == "tilt_fade_out") {
+ r_ret = get_tilt_fade_out(which);
+ } else {
+ return false;
+ }
+ }
+ return true;
+}
+
+void SplineIK3D::_get_property_list(List *p_list) const {
+ LocalVector props;
+ for (uint32_t i = 0; i < settings.size(); i++) {
+ String path = "settings/" + itos(i) + "/";
+ props.push_back(PropertyInfo(Variant::NODE_PATH, path + "path_3d", PROPERTY_HINT_NODE_PATH_VALID_TYPES, "Path3D"));
+ props.push_back(PropertyInfo(Variant::BOOL, path + "tilt_enabled"));
+ props.push_back(PropertyInfo(Variant::INT, path + "tilt_fade_in", PROPERTY_HINT_RANGE, "-1,100,1,or_greater"));
+ props.push_back(PropertyInfo(Variant::INT, path + "tilt_fade_out", PROPERTY_HINT_RANGE, "-1,100,1,or_greater"));
+ }
+
+ for (PropertyInfo &p : props) {
+ _validate_dynamic_prop(p);
+ p_list->push_back(p);
+ }
+
+ ChainIK3D::get_property_list(p_list);
+}
+
+void SplineIK3D::_validate_dynamic_prop(PropertyInfo &p_property) const {
+ PackedStringArray split = p_property.name.split("/");
+ if (split.size() > 2 && split[0] == "settings") {
+ int which = split[1].to_int();
+ if (split[2].begins_with("tilt_") && get_path_3d(which).is_empty()) {
+ p_property.usage = PROPERTY_USAGE_NONE;
+ } else if (split[2].begins_with("tilt_fade_") && !is_tilt_enabled(which)) {
+ p_property.usage = PROPERTY_USAGE_NONE;
+ }
+ }
+}
+
+PackedStringArray SplineIK3D::get_configuration_warnings() const {
+ PackedStringArray warnings = SkeletonModifier3D::get_configuration_warnings();
+ for (uint32_t i = 0; i < sp_settings.size(); i++) {
+ if (sp_settings[i]->path_3d.is_empty()) {
+ warnings.push_back(RTR("Detecting settings with no Path3D set! SplineIK3D must have a Path3D to work."));
+ break;
+ }
+ }
+ return warnings;
+}
+
+// Setting.
+
+void SplineIK3D::set_path_3d(int p_index, const NodePath &p_path_3d) {
+ ERR_FAIL_INDEX(p_index, (int)settings.size());
+ sp_settings[p_index]->path_3d = p_path_3d;
+ notify_property_list_changed();
+ update_configuration_warnings();
+}
+
+NodePath SplineIK3D::get_path_3d(int p_index) const {
+ ERR_FAIL_INDEX_V(p_index, (int)settings.size(), NodePath());
+ return sp_settings[p_index]->path_3d;
+}
+
+void SplineIK3D::set_tilt_enabled(int p_index, bool p_enabled) {
+ ERR_FAIL_INDEX(p_index, (int)settings.size());
+ sp_settings[p_index]->tilt_enabled = p_enabled;
+ notify_property_list_changed();
+}
+
+bool SplineIK3D::is_tilt_enabled(int p_index) const {
+ ERR_FAIL_INDEX_V(p_index, (int)settings.size(), false);
+ return sp_settings[p_index]->tilt_enabled;
+}
+
+void SplineIK3D::set_tilt_fade_in(int p_index, int p_size) {
+ ERR_FAIL_INDEX(p_index, (int)settings.size());
+ sp_settings[p_index]->tilt_fade_in = p_size;
+}
+
+int SplineIK3D::get_tilt_fade_in(int p_index) const {
+ ERR_FAIL_INDEX_V(p_index, (int)settings.size(), -1);
+ return sp_settings[p_index]->tilt_fade_in;
+}
+
+void SplineIK3D::set_tilt_fade_out(int p_index, int p_size) {
+ ERR_FAIL_INDEX(p_index, (int)settings.size());
+ sp_settings[p_index]->tilt_fade_out = p_size;
+}
+
+int SplineIK3D::get_tilt_fade_out(int p_index) const {
+ ERR_FAIL_INDEX_V(p_index, (int)settings.size(), -1);
+ return sp_settings[p_index]->tilt_fade_out;
+}
+
+// Individual joints.
+
+void SplineIK3D::_set_joint_count(int p_index, int p_count) {
+ LocalVector &twists = sp_settings[p_index]->twists;
+ twists.resize(p_count);
+ LocalVector &accum = sp_settings[p_index]->chain_length_accum;
+ accum.resize(p_count);
+}
+
+void SplineIK3D::_bind_methods() {
+ // Setting.
+ ClassDB::bind_method(D_METHOD("set_path_3d", "index", "path_3d"), &SplineIK3D::set_path_3d);
+ ClassDB::bind_method(D_METHOD("get_path_3d", "index"), &SplineIK3D::get_path_3d);
+ ClassDB::bind_method(D_METHOD("set_tilt_enabled", "index", "enabled"), &SplineIK3D::set_tilt_enabled);
+ ClassDB::bind_method(D_METHOD("is_tilt_enabled", "index"), &SplineIK3D::is_tilt_enabled);
+ ClassDB::bind_method(D_METHOD("set_tilt_fade_in", "index", "size"), &SplineIK3D::set_tilt_fade_in);
+ ClassDB::bind_method(D_METHOD("get_tilt_fade_in", "index"), &SplineIK3D::get_tilt_fade_in);
+ ClassDB::bind_method(D_METHOD("set_tilt_fade_out", "index", "size"), &SplineIK3D::set_tilt_fade_out);
+ ClassDB::bind_method(D_METHOD("get_tilt_fade_out", "index"), &SplineIK3D::get_tilt_fade_out);
+
+ ADD_ARRAY_COUNT("Settings", "setting_count", "set_setting_count", "get_setting_count", "settings/");
+}
+
+void SplineIK3D::_init_joints(Skeleton3D *p_skeleton, int p_index) {
+ SplineIK3DSetting *setting = sp_settings[p_index];
+ cached_space = p_skeleton->get_global_transform_interpolated();
+ if (!setting->simulation_dirty) {
+ return;
+ }
+ for (uint32_t i = 0; i < setting->solver_info_list.size(); i++) {
+ if (setting->solver_info_list[i]) {
+ memdelete(setting->solver_info_list[i]);
+ }
+ }
+ setting->solver_info_list.clear();
+ setting->solver_info_list.resize_initialized(setting->joints.size());
+ setting->chain.clear();
+ bool extend_end_bone = setting->extend_end_bone && setting->end_bone_length > 0;
+ double accum = 0.0;
+ for (uint32_t i = 0; i < setting->joints.size(); i++) {
+ setting->chain.push_back(p_skeleton->get_bone_global_pose(setting->joints[i].bone).origin);
+ bool last = i == setting->joints.size() - 1;
+ if (last && extend_end_bone && setting->end_bone_length > 0) {
+ Vector3 axis = get_bone_axis(setting->end_bone.bone, setting->end_bone_direction);
+ if (axis.is_zero_approx()) {
+ setting->chain_length_accum[i] = accum;
+ continue;
+ }
+ setting->solver_info_list[i] = memnew(IKModifier3DSolverInfo);
+ setting->solver_info_list[i]->forward_vector = axis.normalized();
+ setting->solver_info_list[i]->length = setting->end_bone_length;
+ setting->chain.push_back(p_skeleton->get_bone_global_pose(setting->joints[i].bone).xform(axis * setting->end_bone_length));
+ } else if (!last) {
+ Vector3 axis = p_skeleton->get_bone_rest(setting->joints[i + 1].bone).origin;
+ if (axis.is_zero_approx()) {
+ setting->chain_length_accum[i] = accum;
+ continue; // Means always we need to check solver info, but `!solver_info` means that the bone is zero length, so IK should skip it in the all process.
+ }
+ setting->solver_info_list[i] = memnew(IKModifier3DSolverInfo);
+ setting->solver_info_list[i]->forward_vector = axis.normalized();
+ setting->solver_info_list[i]->length = axis.length();
+ }
+ if (setting->solver_info_list[i]) {
+ accum += setting->solver_info_list[i]->length;
+ }
+ setting->chain_length_accum[i] = accum;
+ }
+
+ setting->init_current_joint_rotations(p_skeleton);
+
+ setting->simulation_dirty = false;
+}
+
+void SplineIK3D::_make_simulation_dirty(int p_index) {
+ SplineIK3DSetting *setting = sp_settings[p_index];
+ if (!setting) {
+ return;
+ }
+ setting->simulation_dirty = true;
+}
+
+void SplineIK3D::_process_ik(Skeleton3D *p_skeleton, double p_delta) {
+ for (uint32_t i = 0; i < settings.size(); i++) {
+ _init_joints(p_skeleton, i);
+ if (sp_settings[i]->joints.is_empty()) {
+ continue; // Abort.
+ }
+ Path3D *path_3d = Object::cast_to(get_node_or_null(sp_settings[i]->path_3d));
+ if (!path_3d) {
+ continue; // Abort.
+ }
+ Ref curve = path_3d->get_curve();
+ if (curve.is_null() || curve->get_point_count() == 0) {
+ continue; // Abort.
+ }
+ sp_settings[i]->cache_current_joint_rotations(p_skeleton); // Iterate over first to detect parent (outside of the chain) bone pose changes.
+ _process_joints(p_delta, p_skeleton, sp_settings[i], curve, cached_space.affine_inverse() * path_3d->get_global_transform_interpolated());
+ }
+}
+
+void SplineIK3D::_process_joints(double p_delta, Skeleton3D *p_skeleton, SplineIK3DSetting *p_setting, Ref p_curve, const Transform3D &p_curve_space) {
+ if (p_setting->solver_info_list.is_empty()) {
+ return;
+ }
+ uint32_t joint_count = p_setting->joints.size();
+ uint32_t joint_last = joint_count - 1;
+
+ double path_length = p_curve->get_baked_length();
+ PackedVector3Array points = p_curve->get_baked_points();
+ Vector tilts = p_curve->get_baked_tilts();
+ Vector dists = p_curve->get_baked_dist_cache();
+ uint32_t point_count = points.size();
+ uint32_t point_last = point_count - 1;
+
+ // Make straight segment from root joint to start point.
+ Vector3 start_point = p_curve_space.xform(points[0]);
+ Vector3 start_vector = start_point - p_skeleton->get_bone_global_pose(p_setting->joints[0].bone).origin;
+ double start_dist = start_vector.length();
+
+ // Find first joint on the path.
+ uint32_t chain_path_start = 0;
+ while (chain_path_start < joint_count) {
+ if (p_setting->chain_length_accum[chain_path_start] >= start_dist) {
+ break;
+ }
+ chain_path_start++;
+ }
+ chain_path_start = (uint32_t)CLAMP((int)chain_path_start, 0, (int)joint_last);
+
+ // For tilt fade-in, get bones length not on the path as denominator.
+ double fade_in_denom = 0.0;
+ int denom_start = p_setting->tilt_fade_in > 0 ? CLAMP(p_setting->tilt_fade_in - 1, (int)chain_path_start, (int)joint_count) : -1;
+ int denom_start_to = denom_start - p_setting->tilt_fade_in;
+ if (denom_start >= 0) {
+ for (int i = denom_start; i > denom_start_to; i--) {
+ if (i < 0) {
+ break;
+ }
+ IKModifier3DSolverInfo *solver_info = p_setting->solver_info_list[i];
+ if (!solver_info || Math::is_zero_approx(solver_info->length)) {
+ continue;
+ }
+ fade_in_denom += solver_info->length;
+ }
+ }
+
+ // Prepare for fade-out.
+ uint32_t ended = 0;
+ Vector3 end_point = p_curve_space.xform(points[point_last]);
+ Vector3 end_vector;
+ double end_to_end_length = 0.0;
+ double fade_out_denom = 0.0;
+
+ uint32_t last_nearest = 0;
+ uint32_t last_nearest_next = 0;
+ double last_interpolate = 0.0;
+
+ for (uint32_t i = 0; i < p_setting->solver_info_list.size(); i++) {
+ IKModifier3DSolverInfo *solver_info = p_setting->solver_info_list[i];
+ if (!solver_info || Math::is_zero_approx(solver_info->length)) {
+ continue;
+ }
+ uint32_t HEAD = i;
+ uint32_t TAIL = i + 1;
+
+ bool is_fitting_first = HEAD == chain_path_start;
+
+ // Special case for out of path joints.
+ if (point_count == 1 || HEAD <= chain_path_start) {
+ // Set twist only for first fitting joint.
+ if (!is_fitting_first) {
+ p_setting->update_chain_coordinate(p_skeleton, TAIL, limit_length(p_setting->chain[HEAD], p_setting->chain[HEAD] + start_vector, solver_info->length));
+ }
+ if (p_setting->tilt_enabled) {
+ if (p_setting->tilt_fade_in < 0) {
+ p_setting->twists[HEAD] = 0.0;
+ } else if (p_setting->tilt_fade_in == 0) {
+ p_setting->twists[HEAD] = tilts[0];
+ } else {
+ // Decreases monotonically in a straight line, fetch the distance.
+ double fade_in_dumping = CLAMP((double)(p_setting->chain[HEAD].distance_to(start_point) / fade_in_denom), 0.0, 1.0);
+ p_setting->twists[HEAD] = Math::lerp((double)tilts[0], 0.0, fade_in_dumping);
+ }
+ }
+ if (!is_fitting_first) {
+ continue;
+ }
+ } else if (ended > 0) {
+ p_setting->update_chain_coordinate(p_skeleton, TAIL, limit_length(p_setting->chain[HEAD], p_setting->chain[HEAD] + end_vector, solver_info->length));
+ if (p_setting->tilt_enabled) {
+ if (p_setting->tilt_fade_out < 0) {
+ p_setting->twists[HEAD] = 0.0;
+ } else if (p_setting->tilt_fade_out == 0) {
+ p_setting->twists[HEAD] = tilts[point_last];
+ } else {
+ // Increases monotonically in a bended line, accumulate the distances.
+ if (ended == 1) {
+ end_to_end_length = p_setting->chain[TAIL].distance_to(end_point);
+ } else {
+ end_to_end_length += solver_info->length;
+ }
+ double fade_out_dumping = CLAMP(end_to_end_length / fade_out_denom, 0.0, 1.0);
+ p_setting->twists[HEAD] = Math::lerp(ended == 1 ? Math::lerp((double)tilts[last_nearest], (double)tilts[last_nearest_next], last_interpolate) : (double)tilts[point_last], 0.0, fade_out_dumping);
+ ended = 2;
+ }
+ }
+ continue;
+ }
+
+ // General case.
+ double lsq = solver_info->length * solver_info->length;
+ Vector3 head_in_chain_space = p_curve_space.xform_inv(p_setting->chain[HEAD]);
+ double interpolate = 0.0;
+ uint32_t nearest = p_setting->find_nearest_point(head_in_chain_space, lsq, points, p_curve->is_closed(), last_nearest, &interpolate);
+ if (nearest >= point_count) {
+ if (HEAD == 0) {
+ nearest = point_count - 2;
+ interpolate = 1.0;
+ } else {
+ Vector3 chain_end = (p_setting->chain[HEAD] - p_setting->chain[HEAD - 1]).normalized();
+ Vector3 path_end = (p_curve_space.xform(points[point_last]) - p_setting->chain[HEAD]).normalized();
+ double rest_path_length = path_length - Math::lerp((double)dists[last_nearest], (double)dists[last_nearest_next], last_interpolate);
+ interpolate = CLAMP(rest_path_length / solver_info->length, 0.0, 1.0); // End vector should be defined only one end bone to make neat interpolating.
+ end_vector = chain_end.lerp(path_end, interpolate);
+
+ int denom_end = p_setting->tilt_fade_out > 0 ? CLAMP((int)joint_last - p_setting->tilt_fade_out, 0, (int)last_nearest) : -1;
+ int denom_end_to = denom_end + p_setting->tilt_fade_out;
+ if (denom_end >= 0) {
+ for (int e = denom_end; e < denom_end_to; e++) {
+ if (e >= (int)joint_count) {
+ break;
+ }
+ IKModifier3DSolverInfo *end_solver_info = p_setting->solver_info_list[e];
+ if (!end_solver_info || Math::is_zero_approx(end_solver_info->length)) {
+ continue;
+ }
+ fade_out_denom += end_solver_info->length;
+ }
+ }
+
+ ended = 1;
+ i--; // Will be processed above special case.
+ continue;
+ }
+ }
+ uint32_t nearest_next = p_curve->is_closed() ? Math::posmod(nearest + 1, point_count) : CLAMP(nearest, (uint32_t)0, point_last);
+ p_setting->update_chain_coordinate(p_skeleton, TAIL, limit_length(p_setting->chain[HEAD], p_curve_space.xform(points[nearest].lerp(points[nearest_next], interpolate)), solver_info->length));
+ if (!is_fitting_first) {
+ p_setting->twists[HEAD] = Math::lerp((double)tilts[last_nearest], (double)tilts[last_nearest_next], last_interpolate);
+ }
+ last_nearest = nearest;
+ last_nearest_next = nearest_next;
+ last_interpolate = interpolate;
+ }
+
+ // Update virtual bone rest/poses.
+ p_setting->cache_current_joint_rotations(p_skeleton, p_setting->tilt_enabled); // Pass p_setting->tilt_enabled to skip unneeded rotate process.
+
+ // Apply the virtual bone rest/poses to the actual bones.
+ for (uint32_t i = 0; i < p_setting->solver_info_list.size(); i++) {
+ IKModifier3DSolverInfo *solver_info = p_setting->solver_info_list[i];
+ if (!solver_info || Math::is_zero_approx(solver_info->length)) {
+ continue;
+ }
+ p_skeleton->set_bone_pose_rotation(p_setting->joints[i].bone, solver_info->current_lpose);
+ }
+}
+
+SplineIK3D::~SplineIK3D() {
+ clear_settings();
+}
diff --git a/scene/3d/spline_ik_3d.h b/scene/3d/spline_ik_3d.h
new file mode 100644
index 00000000000..cd46b58e03b
--- /dev/null
+++ b/scene/3d/spline_ik_3d.h
@@ -0,0 +1,187 @@
+/**************************************************************************/
+/* spline_ik_3d.h */
+/**************************************************************************/
+/* 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. */
+/**************************************************************************/
+
+#pragma once
+
+#include "scene/3d/chain_ik_3d.h"
+#include "scene/3d/path_3d.h"
+
+class SplineIK3D : public ChainIK3D {
+ GDCLASS(SplineIK3D, ChainIK3D);
+
+public:
+ struct SplineIK3DSetting : public ChainIK3DSetting {
+ NodePath path_3d;
+ bool tilt_enabled = true;
+ int tilt_fade_in = 1;
+ int tilt_fade_out = 1;
+
+ LocalVector chain_length_accum;
+ LocalVector twists;
+
+ // Find the nearest point.
+ int32_t find_nearest_point(const Vector3 &p_origin, double p_length_sq, const PackedVector3Array &p_points, bool p_is_path_closed, uint32_t p_first, double *r_ret) {
+ ERR_FAIL_COND_V(p_first >= p_points.size(), p_points.size() - 1);
+ uint32_t i = p_first;
+ if (p_is_path_closed) {
+ while (true) {
+ if (p_origin.distance_squared_to(p_points[i]) >= p_length_sq) {
+ break;
+ }
+ i++;
+ i = Math::posmod(i, p_points.size());
+ if (i == p_first) {
+ i = p_points.size();
+ break; // Can't found, use last.
+ }
+ }
+ } else {
+ while (i < p_points.size()) {
+ if (p_origin.distance_squared_to(p_points[i]) >= p_length_sq) {
+ break;
+ }
+ i++;
+ }
+ }
+ if (i == 0) {
+ *r_ret = 0.0;
+ return p_first;
+ } else if (i >= p_points.size()) {
+ *r_ret = 1.0;
+ return i;
+ }
+ *r_ret = Math::inverse_lerp((double)p_origin.distance_squared_to(p_points[i - 1]), (double)p_origin.distance_squared_to(p_points[i]), p_length_sq);
+ return i - 1;
+ }
+
+ // Make rotation as bone pose from chain coordinates.
+ void cache_current_joint_rotations(Skeleton3D *p_skeleton, bool p_use_tilt = false) {
+ Transform3D parent_gpose_tr;
+ int parent = p_skeleton->get_bone_parent(root_bone.bone);
+ if (parent >= 0) {
+ parent_gpose_tr = p_skeleton->get_bone_global_pose(parent);
+ }
+ Quaternion parent_gpose = parent_gpose_tr.basis.get_rotation_quaternion();
+
+ if (p_use_tilt) {
+ double parent_twist = 0.0;
+ for (uint32_t i = 0; i < joints.size(); i++) {
+ int HEAD = i;
+ IKModifier3DSolverInfo *solver_info = solver_info_list[HEAD];
+ if (!solver_info) {
+ continue;
+ }
+ solver_info->current_lrest = p_skeleton->get_bone_pose(joints[HEAD].bone).basis.get_rotation_quaternion();
+ solver_info->current_grest = parent_gpose * solver_info->current_lrest;
+ solver_info->current_grest.normalize();
+ Vector3 from = solver_info->forward_vector;
+ Vector3 to = solver_info->current_grest.xform_inv(solver_info->current_vector).normalized();
+
+ Basis b = get_swing(Quaternion(from, to), from);
+ b.rotate_local(from, twists[HEAD] - parent_twist);
+ parent_twist = twists[HEAD];
+ solver_info->current_lpose = solver_info->current_lrest * b.get_rotation_quaternion();
+
+ solver_info->current_gpose = parent_gpose * solver_info->current_lpose;
+ solver_info->current_gpose.normalize();
+ parent_gpose = solver_info->current_gpose;
+ }
+ } else {
+ for (uint32_t i = 0; i < joints.size(); i++) {
+ int HEAD = i;
+ IKModifier3DSolverInfo *solver_info = solver_info_list[HEAD];
+ if (!solver_info) {
+ continue;
+ }
+ solver_info->current_lrest = p_skeleton->get_bone_pose(joints[HEAD].bone).basis.get_rotation_quaternion();
+ solver_info->current_grest = parent_gpose * solver_info->current_lrest;
+ solver_info->current_grest.normalize();
+ Vector3 from = solver_info->forward_vector;
+ Vector3 to = solver_info->current_grest.xform_inv(solver_info->current_vector).normalized();
+
+ solver_info->current_lpose = solver_info->current_lrest * get_swing(Quaternion(from, to), from);
+
+ solver_info->current_gpose = parent_gpose * solver_info->current_lpose;
+ solver_info->current_gpose.normalize();
+ parent_gpose = solver_info->current_gpose;
+ }
+ }
+
+ // To update positions in preprocess of _process_joints().
+ cache_current_vectors(p_skeleton);
+ }
+ };
+
+protected:
+ LocalVector sp_settings; // For caching.
+
+ bool _get(const StringName &p_path, Variant &r_ret) const;
+ bool _set(const StringName &p_path, const Variant &p_value);
+ void _get_property_list(List *p_list) const;
+ void _validate_dynamic_prop(PropertyInfo &p_property) const;
+
+ static void _bind_methods();
+
+ virtual void _init_joints(Skeleton3D *p_skeleton, int p_index) override;
+ virtual void _make_simulation_dirty(int p_index) override;
+
+ virtual void _process_ik(Skeleton3D *p_skeleton, double p_delta) override;
+ void _process_joints(double p_delta, Skeleton3D *p_skeleton, SplineIK3DSetting *p_setting, Ref p_curve, const Transform3D &p_curve_space);
+
+ virtual void _set_joint_count(int p_index, int p_count) override;
+
+public:
+ virtual PackedStringArray get_configuration_warnings() const override;
+ virtual void set_setting_count(int p_count) override {
+ _set_setting_count(p_count);
+ sp_settings = _cast_settings();
+ chain_settings = _cast_settings(); // Don't forget to sync super class settings.
+ }
+ virtual void clear_settings() override {
+ _set_setting_count(0);
+ sp_settings.clear();
+ chain_settings.clear(); // Don't forget to sync super class settings.
+ }
+
+ // Setting.
+ void set_path_3d(int p_index, const NodePath &p_path_3d);
+ NodePath get_path_3d(int p_index) const;
+ void set_tilt_enabled(int p_index, bool p_enabled);
+ bool is_tilt_enabled(int p_index) const;
+ void set_tilt_fade_in(int p_index, int p_size);
+ int get_tilt_fade_in(int p_index) const;
+ void set_tilt_fade_out(int p_index, int p_size);
+ int get_tilt_fade_out(int p_index) const;
+
+ // Helper.
+ double get_bezier_arc_length();
+
+ ~SplineIK3D();
+};
diff --git a/scene/3d/spring_bone_simulator_3d.compat.inc b/scene/3d/spring_bone_simulator_3d.compat.inc
new file mode 100644
index 00000000000..4fa1e94caf5
--- /dev/null
+++ b/scene/3d/spring_bone_simulator_3d.compat.inc
@@ -0,0 +1,89 @@
+/**************************************************************************/
+/* spring_bone_simulator_3d.compat.inc */
+/**************************************************************************/
+/* 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. */
+/**************************************************************************/
+
+#ifndef DISABLE_DEPRECATED
+
+namespace compat::SpringBoneSimulator3D {
+enum BoneDirection : int {
+ BONE_DIRECTION_PLUS_X = SkeletonModifier3D::BONE_DIRECTION_PLUS_X,
+ BONE_DIRECTION_MINUS_X = SkeletonModifier3D::BONE_DIRECTION_MINUS_X,
+ BONE_DIRECTION_PLUS_Y = SkeletonModifier3D::BONE_DIRECTION_PLUS_Y,
+ BONE_DIRECTION_MINUS_Y = SkeletonModifier3D::BONE_DIRECTION_MINUS_Y,
+ BONE_DIRECTION_PLUS_Z = SkeletonModifier3D::BONE_DIRECTION_PLUS_Z,
+ BONE_DIRECTION_MINUS_Z = SkeletonModifier3D::BONE_DIRECTION_MINUS_Z,
+ BONE_DIRECTION_FROM_PARENT = SkeletonModifier3D::BONE_DIRECTION_FROM_PARENT,
+};
+
+enum RotationAxis : int {
+ ROTATION_AXIS_X = SkeletonModifier3D::ROTATION_AXIS_X,
+ ROTATION_AXIS_Y = SkeletonModifier3D::ROTATION_AXIS_Y,
+ ROTATION_AXIS_Z = SkeletonModifier3D::ROTATION_AXIS_Z,
+ ROTATION_AXIS_ALL = SkeletonModifier3D::ROTATION_AXIS_ALL,
+ ROTATION_AXIS_CUSTOM = SkeletonModifier3D::ROTATION_AXIS_CUSTOM,
+};
+} //namespace compat::SpringBoneSimulator3D
+
+VARIANT_ENUM_CAST(compat::SpringBoneSimulator3D::BoneDirection);
+VARIANT_ENUM_CAST(compat::SpringBoneSimulator3D::RotationAxis);
+
+compat::SpringBoneSimulator3D::BoneDirection SpringBoneSimulator3D::_get_end_bone_direction_bind_compat_110120(int p_index) const {
+ return static_cast((int)get_end_bone_direction(p_index));
+}
+
+void SpringBoneSimulator3D::_set_end_bone_direction_bind_compat_110120(int p_index, compat::SpringBoneSimulator3D::BoneDirection p_bone_direction) {
+ set_end_bone_direction(p_index, static_cast((int)p_bone_direction));
+}
+
+compat::SpringBoneSimulator3D::RotationAxis SpringBoneSimulator3D::_get_rotation_axis_bind_compat_110120(int p_index) const {
+ return static_cast((int)get_rotation_axis(p_index));
+}
+
+void SpringBoneSimulator3D::_set_rotation_axis_bind_compat_110120(int p_index, compat::SpringBoneSimulator3D::RotationAxis p_axis) {
+ set_rotation_axis(p_index, static_cast((int)p_axis));
+}
+
+compat::SpringBoneSimulator3D::RotationAxis SpringBoneSimulator3D::_get_joint_rotation_axis_bind_compat_110120(int p_index, int p_joint) const {
+ return static_cast((int)get_joint_rotation_axis(p_index, p_joint));
+}
+
+void SpringBoneSimulator3D::_set_joint_rotation_axis_bind_compat_110120(int p_index, int p_joint, compat::SpringBoneSimulator3D::RotationAxis p_axis) {
+ set_joint_rotation_axis(p_index, p_joint, static_cast((int)p_axis));
+}
+
+void SpringBoneSimulator3D::_bind_compatibility_methods() {
+ ClassDB::bind_compatibility_method(D_METHOD("get_end_bone_direction", "index"), &SpringBoneSimulator3D::_get_end_bone_direction_bind_compat_110120);
+ ClassDB::bind_compatibility_method(D_METHOD("set_end_bone_direction", "index", "bone_direction"), &SpringBoneSimulator3D::_set_end_bone_direction_bind_compat_110120);
+ ClassDB::bind_compatibility_method(D_METHOD("get_rotation_axis", "index"), &SpringBoneSimulator3D::_get_rotation_axis_bind_compat_110120);
+ ClassDB::bind_compatibility_method(D_METHOD("set_rotation_axis", "index", "axis"), &SpringBoneSimulator3D::_set_rotation_axis_bind_compat_110120);
+ ClassDB::bind_compatibility_method(D_METHOD("get_joint_rotation_axis", "index", "joint"), &SpringBoneSimulator3D::_get_joint_rotation_axis_bind_compat_110120);
+ ClassDB::bind_compatibility_method(D_METHOD("set_joint_rotation_axis", "index", "joint", "axis"), &SpringBoneSimulator3D::_set_joint_rotation_axis_bind_compat_110120);
+}
+
+#endif // DISABLE_DEPRECATED
diff --git a/scene/3d/spring_bone_simulator_3d.cpp b/scene/3d/spring_bone_simulator_3d.cpp
index 72dfeff4b52..d84877d370e 100644
--- a/scene/3d/spring_bone_simulator_3d.cpp
+++ b/scene/3d/spring_bone_simulator_3d.cpp
@@ -29,6 +29,7 @@
/**************************************************************************/
#include "spring_bone_simulator_3d.h"
+#include "spring_bone_simulator_3d.compat.inc"
#include "scene/3d/spring_bone_collision_3d.h"
@@ -298,14 +299,14 @@ void SpringBoneSimulator3D::_get_property_list(List *p_list) const
props.push_back(PropertyInfo(Variant::STRING, path + "end_bone_name", PROPERTY_HINT_ENUM_SUGGESTION, enum_hint));
props.push_back(PropertyInfo(Variant::INT, path + "end_bone", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_NO_EDITOR));
props.push_back(PropertyInfo(Variant::BOOL, path + "extend_end_bone"));
- props.push_back(PropertyInfo(Variant::INT, path + "end_bone/direction", PROPERTY_HINT_ENUM, "+X,-X,+Y,-Y,+Z,-Z,FromParent"));
+ props.push_back(PropertyInfo(Variant::INT, path + "end_bone/direction", PROPERTY_HINT_ENUM, SkeletonModifier3D::get_hint_bone_direction()));
props.push_back(PropertyInfo(Variant::FLOAT, path + "end_bone/length", PROPERTY_HINT_RANGE, "0,1,0.001,or_greater,suffix:m"));
props.push_back(PropertyInfo(Variant::INT, path + "center_from", PROPERTY_HINT_ENUM, "WorldOrigin,Node,Bone"));
props.push_back(PropertyInfo(Variant::NODE_PATH, path + "center_node"));
props.push_back(PropertyInfo(Variant::STRING, path + "center_bone_name", PROPERTY_HINT_ENUM_SUGGESTION, enum_hint));
props.push_back(PropertyInfo(Variant::INT, path + "center_bone", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_NO_EDITOR));
props.push_back(PropertyInfo(Variant::BOOL, path + "individual_config"));
- props.push_back(PropertyInfo(Variant::INT, path + "rotation_axis", PROPERTY_HINT_ENUM, "X,Y,Z,All,Custom"));
+ props.push_back(PropertyInfo(Variant::INT, path + "rotation_axis", PROPERTY_HINT_ENUM, SkeletonModifier3D::get_hint_rotation_axis()));
props.push_back(PropertyInfo(Variant::VECTOR3, path + "rotation_axis_vector"));
props.push_back(PropertyInfo(Variant::FLOAT, path + "radius/value", PROPERTY_HINT_RANGE, "0,1,0.001,or_greater,suffix:m"));
props.push_back(PropertyInfo(Variant::OBJECT, path + "radius/damping_curve", PROPERTY_HINT_RESOURCE_TYPE, "Curve"));
@@ -319,9 +320,9 @@ void SpringBoneSimulator3D::_get_property_list(List *p_list) const
props.push_back(PropertyInfo(Variant::INT, path + "joint_count", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_DEFAULT | PROPERTY_USAGE_ARRAY, "Joints," + path + "joints/,static,const"));
for (uint32_t j = 0; j < settings[i]->joints.size(); j++) {
String joint_path = path + "joints/" + itos(j) + "/";
- props.push_back(PropertyInfo(Variant::STRING, joint_path + "bone_name", PROPERTY_HINT_ENUM_SUGGESTION, enum_hint, PROPERTY_USAGE_EDITOR | PROPERTY_USAGE_READ_ONLY | PROPERTY_USAGE_STORAGE));
+ props.push_back(PropertyInfo(Variant::STRING, joint_path + "bone_name", PROPERTY_HINT_ENUM_SUGGESTION, enum_hint, PROPERTY_USAGE_EDITOR | PROPERTY_USAGE_READ_ONLY));
props.push_back(PropertyInfo(Variant::INT, joint_path + "bone", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_NO_EDITOR | PROPERTY_USAGE_READ_ONLY));
- props.push_back(PropertyInfo(Variant::INT, joint_path + "rotation_axis", PROPERTY_HINT_ENUM, "X,Y,Z,All,Custom"));
+ props.push_back(PropertyInfo(Variant::INT, joint_path + "rotation_axis", PROPERTY_HINT_ENUM, SkeletonModifier3D::get_hint_rotation_axis()));
props.push_back(PropertyInfo(Variant::VECTOR3, joint_path + "rotation_axis_vector"));
props.push_back(PropertyInfo(Variant::FLOAT, joint_path + "radius", PROPERTY_HINT_RANGE, "0,1,0.001,or_greater,suffix:m"));
props.push_back(PropertyInfo(Variant::FLOAT, joint_path + "stiffness", PROPERTY_HINT_RANGE, "0,4,0.01,or_greater"));
@@ -354,7 +355,12 @@ void SpringBoneSimulator3D::_validate_dynamic_prop(PropertyInfo &p_property) con
int which = split[1].to_int();
// Extended end bone option.
- if (split[2] == "end_bone" && !is_end_bone_extended(which) && split.size() > 3) {
+ bool force_hide = false;
+ if (split[2] == "extend_end_bone" && get_end_bone(which) == -1) {
+ p_property.usage = PROPERTY_USAGE_NONE;
+ force_hide = true;
+ }
+ if (force_hide || (split[2] == "end_bone" && !is_end_bone_extended(which) && split.size() > 3)) {
p_property.usage = PROPERTY_USAGE_NONE;
}
@@ -502,6 +508,7 @@ void SpringBoneSimulator3D::set_end_bone(int p_index, int p_bone) {
if (changed) {
_update_joint_array(p_index);
}
+ notify_property_list_changed();
}
int SpringBoneSimulator3D::get_end_bone(int p_index) const {
@@ -527,7 +534,7 @@ void SpringBoneSimulator3D::set_end_bone_direction(int p_index, BoneDirection p_
_make_joints_dirty(p_index);
}
-SpringBoneSimulator3D::BoneDirection SpringBoneSimulator3D::get_end_bone_direction(int p_index) const {
+SkeletonModifier3D::BoneDirection SpringBoneSimulator3D::get_end_bone_direction(int p_index) const {
ERR_FAIL_INDEX_V(p_index, (int)settings.size(), BONE_DIRECTION_FROM_PARENT);
return settings[p_index]->end_bone_direction;
}
@@ -784,7 +791,7 @@ void SpringBoneSimulator3D::set_rotation_axis(int p_index, RotationAxis p_axis)
notify_property_list_changed();
}
-SpringBoneSimulator3D::RotationAxis SpringBoneSimulator3D::get_rotation_axis(int p_index) const {
+SkeletonModifier3D::RotationAxis SpringBoneSimulator3D::get_rotation_axis(int p_index) const {
ERR_FAIL_INDEX_V(p_index, (int)settings.size(), ROTATION_AXIS_ALL);
return settings[p_index]->rotation_axis;
}
@@ -828,6 +835,7 @@ void SpringBoneSimulator3D::set_setting_count(int p_count) {
if (delta < 0) {
for (int i = delta; i < 0; i++) {
memdelete(settings[(int)settings.size() + i]);
+ settings[(int)settings.size() + i] = nullptr;
}
}
settings.resize(p_count);
@@ -863,6 +871,7 @@ bool SpringBoneSimulator3D::is_config_individual(int p_index) const {
}
void SpringBoneSimulator3D::set_joint_bone_name(int p_index, int p_joint, const String &p_bone_name) {
+ // Exists only for indicate bone name on the inspector, no needs to make dirty joint array.
ERR_FAIL_INDEX(p_index, (int)settings.size());
LocalVector &joints = settings[p_index]->joints;
ERR_FAIL_INDEX(p_joint, (int)joints.size());
@@ -1011,7 +1020,7 @@ void SpringBoneSimulator3D::set_joint_rotation_axis(int p_index, int p_joint, Ro
#endif // TOOLS_ENABLED
}
-SpringBoneSimulator3D::RotationAxis SpringBoneSimulator3D::get_joint_rotation_axis(int p_index, int p_joint) const {
+SkeletonModifier3D::RotationAxis SpringBoneSimulator3D::get_joint_rotation_axis(int p_index, int p_joint) const {
ERR_FAIL_INDEX_V(p_index, (int)settings.size(), ROTATION_AXIS_ALL);
const LocalVector &joints = settings[p_index]->joints;
ERR_FAIL_INDEX_V(p_joint, (int)joints.size(), ROTATION_AXIS_ALL);
@@ -1051,6 +1060,7 @@ void SpringBoneSimulator3D::set_joint_count(int p_index, int p_count) {
if (delta < 0) {
for (int i = delta; i < 0; i++) {
memdelete(joints[joints.size() + i]);
+ joints[joints.size() + i] = nullptr;
}
}
joints.resize(p_count);
@@ -1315,23 +1325,9 @@ void SpringBoneSimulator3D::_bind_methods() {
ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "external_force", PROPERTY_HINT_RANGE, "-99999,99999,or_greater,or_less,hide_control,suffix:m/s"), "set_external_force", "get_external_force");
ADD_ARRAY_COUNT("Settings", "setting_count", "set_setting_count", "get_setting_count", "settings/");
- BIND_ENUM_CONSTANT(BONE_DIRECTION_PLUS_X);
- BIND_ENUM_CONSTANT(BONE_DIRECTION_MINUS_X);
- BIND_ENUM_CONSTANT(BONE_DIRECTION_PLUS_Y);
- BIND_ENUM_CONSTANT(BONE_DIRECTION_MINUS_Y);
- BIND_ENUM_CONSTANT(BONE_DIRECTION_PLUS_Z);
- BIND_ENUM_CONSTANT(BONE_DIRECTION_MINUS_Z);
- BIND_ENUM_CONSTANT(BONE_DIRECTION_FROM_PARENT);
-
BIND_ENUM_CONSTANT(CENTER_FROM_WORLD_ORIGIN);
BIND_ENUM_CONSTANT(CENTER_FROM_NODE);
BIND_ENUM_CONSTANT(CENTER_FROM_BONE);
-
- BIND_ENUM_CONSTANT(ROTATION_AXIS_X);
- BIND_ENUM_CONSTANT(ROTATION_AXIS_Y);
- BIND_ENUM_CONSTANT(ROTATION_AXIS_Z);
- BIND_ENUM_CONSTANT(ROTATION_AXIS_ALL);
- BIND_ENUM_CONSTANT(ROTATION_AXIS_CUSTOM);
}
void SpringBoneSimulator3D::_skeleton_changed(Skeleton3D *p_old, Skeleton3D *p_new) {
@@ -1420,7 +1416,7 @@ void SpringBoneSimulator3D::_validate_rotation_axis(Skeleton3D *p_skeleton, int
}
fwd.normalize();
if (Math::is_equal_approx(Math::abs(rot.dot(fwd)), 1)) {
- WARN_PRINT_ED("Setting: " + itos(p_index) + " Joint: " + itos(p_joint) + ": Rotation axis and forward vectors are colinear. This is not advised as it may cause unwanted rotation.");
+ WARN_PRINT_ED("Setting: " + itos(p_index) + " Joint: " + itos(p_joint) + ": Rotation axis and forward vector are colinear. This is not advised as it may cause unwanted rotation.");
}
}
@@ -1492,9 +1488,6 @@ void SpringBoneSimulator3D::_find_collisions() {
}
void SpringBoneSimulator3D::_process_collisions() {
- if (!is_inside_tree()) {
- return;
- }
for (const ObjectID &oid : collisions) {
Object *t_obj = ObjectDB::get_instance(oid);
if (!t_obj) {
@@ -1535,7 +1528,7 @@ void SpringBoneSimulator3D::_update_joint_array(int p_index) {
if (!valid) {
set_joint_count(p_index, 0);
- ERR_FAIL_EDMSG("End bone must be the same as or a child of root bone.");
+ ERR_FAIL_EDMSG("End bone must be the same as or a child of the root bone.");
}
LocalVector new_joints;
@@ -1619,10 +1612,6 @@ void SpringBoneSimulator3D::_set_active(bool p_active) {
}
void SpringBoneSimulator3D::_process_modification(double p_delta) {
- if (!is_inside_tree()) {
- return;
- }
-
Skeleton3D *skeleton = get_skeleton();
if (!skeleton) {
return;
@@ -1634,7 +1623,7 @@ void SpringBoneSimulator3D::_process_modification(double p_delta) {
if (saving) {
return; // Collision position has been reset but we don't want to process simulating on saving. Abort.
}
-#endif //TOOLS_ENABLED
+#endif // TOOLS_ENABLED
for (uint32_t i = 0; i < settings.size(); i++) {
_init_joints(skeleton, settings[i]);
diff --git a/scene/3d/spring_bone_simulator_3d.h b/scene/3d/spring_bone_simulator_3d.h
index f91b035bf2d..e5fe2296ca3 100644
--- a/scene/3d/spring_bone_simulator_3d.h
+++ b/scene/3d/spring_bone_simulator_3d.h
@@ -32,12 +32,19 @@
#include "scene/3d/skeleton_modifier_3d.h"
+#ifndef DISABLE_DEPRECATED
+namespace compat::SpringBoneSimulator3D {
+enum BoneDirection : int;
+enum RotationAxis : int;
+} //namespace compat::SpringBoneSimulator3D
+#endif
+
class SpringBoneSimulator3D : public SkeletonModifier3D {
GDCLASS(SpringBoneSimulator3D, SkeletonModifier3D);
#ifdef TOOLS_ENABLED
bool saving = false;
-#endif //TOOLS_ENABLED
+#endif // TOOLS_ENABLED
bool joints_dirty = false;
@@ -48,30 +55,12 @@ class SpringBoneSimulator3D : public SkeletonModifier3D {
void _make_collisions_dirty();
public:
- enum BoneDirection {
- BONE_DIRECTION_PLUS_X,
- BONE_DIRECTION_MINUS_X,
- BONE_DIRECTION_PLUS_Y,
- BONE_DIRECTION_MINUS_Y,
- BONE_DIRECTION_PLUS_Z,
- BONE_DIRECTION_MINUS_Z,
- BONE_DIRECTION_FROM_PARENT,
- };
-
enum CenterFrom {
CENTER_FROM_WORLD_ORIGIN,
CENTER_FROM_NODE,
CENTER_FROM_BONE,
};
- enum RotationAxis {
- ROTATION_AXIS_X,
- ROTATION_AXIS_Y,
- ROTATION_AXIS_Z,
- ROTATION_AXIS_ALL,
- ROTATION_AXIS_CUSTOM,
- };
-
struct SpringBone3DVerletInfo {
Vector3 prev_tail;
Vector3 current_tail;
@@ -198,6 +187,17 @@ protected:
void _validate_rotation_axes(Skeleton3D *p_skeleton) const;
void _validate_rotation_axis(Skeleton3D *p_skeleton, int p_index, int p_joint) const;
+#ifndef DISABLE_DEPRECATED
+ compat::SpringBoneSimulator3D::BoneDirection _get_end_bone_direction_bind_compat_110120(int p_index) const;
+ void _set_end_bone_direction_bind_compat_110120(int p_index, compat::SpringBoneSimulator3D::BoneDirection p_bone_direction);
+ compat::SpringBoneSimulator3D::RotationAxis _get_rotation_axis_bind_compat_110120(int p_index) const;
+ void _set_rotation_axis_bind_compat_110120(int p_index, compat::SpringBoneSimulator3D::RotationAxis p_axis);
+ compat::SpringBoneSimulator3D::RotationAxis _get_joint_rotation_axis_bind_compat_110120(int p_index, int p_joint) const;
+ void _set_joint_rotation_axis_bind_compat_110120(int p_index, int p_joint, compat::SpringBoneSimulator3D::RotationAxis p_axis);
+
+ static void _bind_compatibility_methods();
+#endif // DISABLE_DEPRECATED
+
public:
// Setting.
void set_root_bone_name(int p_index, const String &p_bone_name);
@@ -314,6 +314,4 @@ public:
~SpringBoneSimulator3D();
};
-VARIANT_ENUM_CAST(SpringBoneSimulator3D::BoneDirection);
VARIANT_ENUM_CAST(SpringBoneSimulator3D::CenterFrom);
-VARIANT_ENUM_CAST(SpringBoneSimulator3D::RotationAxis);
diff --git a/scene/3d/two_bone_ik_3d.cpp b/scene/3d/two_bone_ik_3d.cpp
new file mode 100644
index 00000000000..1a6b2bc2abd
--- /dev/null
+++ b/scene/3d/two_bone_ik_3d.cpp
@@ -0,0 +1,778 @@
+/**************************************************************************/
+/* two_bone_ik_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 "two_bone_ik_3d.h"
+
+bool TwoBoneIK3D::_set(const StringName &p_path, const Variant &p_value) {
+ String path = p_path;
+
+ if (path.begins_with("settings/")) {
+ int which = path.get_slicec('/', 1).to_int();
+ String what = path.get_slicec('/', 2);
+ ERR_FAIL_INDEX_V(which, (int)settings.size(), false);
+
+ if (what == "target_node") {
+ set_target_node(which, p_value);
+ } else if (what == "pole_node") {
+ set_pole_node(which, p_value);
+ } else if (what == "root_bone_name") {
+ set_root_bone_name(which, p_value);
+ } else if (what == "root_bone") {
+ set_root_bone(which, p_value);
+ } else if (what == "middle_bone_name") {
+ set_middle_bone_name(which, p_value);
+ } else if (what == "middle_bone") {
+ set_middle_bone(which, p_value);
+ } else if (what == "pole_direction") {
+ set_pole_direction(which, static_cast((int)p_value));
+ } else if (what == "pole_direction_vector") {
+ set_pole_direction_vector(which, p_value);
+ } else if (what == "end_bone_name") {
+ set_end_bone_name(which, p_value);
+ } else if (what == "end_bone") {
+ String opt = path.get_slicec('/', 3);
+ if (opt.is_empty()) {
+ set_end_bone(which, p_value);
+ } else if (opt == "direction") {
+ set_end_bone_direction(which, static_cast((int)p_value));
+ } else if (opt == "length") {
+ set_end_bone_length(which, p_value);
+ } else {
+ return false;
+ }
+ } else if (what == "use_virtual_end") {
+ set_use_virtual_end(which, p_value);
+ } else if (what == "extend_end_bone") {
+ set_extend_end_bone(which, p_value);
+ } else {
+ return false;
+ }
+ }
+ return true;
+}
+
+bool TwoBoneIK3D::_get(const StringName &p_path, Variant &r_ret) const {
+ String path = p_path;
+
+ if (path.begins_with("settings/")) {
+ int which = path.get_slicec('/', 1).to_int();
+ String what = path.get_slicec('/', 2);
+ ERR_FAIL_INDEX_V(which, (int)settings.size(), false);
+
+ if (what == "target_node") {
+ r_ret = get_target_node(which);
+ } else if (what == "pole_node") {
+ r_ret = get_pole_node(which);
+ } else if (what == "root_bone_name") {
+ r_ret = get_root_bone_name(which);
+ } else if (what == "root_bone") {
+ r_ret = get_root_bone(which);
+ } else if (what == "middle_bone_name") {
+ r_ret = get_middle_bone_name(which);
+ } else if (what == "middle_bone") {
+ r_ret = get_middle_bone(which);
+ } else if (what == "pole_direction") {
+ r_ret = (int)get_pole_direction(which);
+ } else if (what == "pole_direction_vector") {
+ r_ret = get_pole_direction_vector(which);
+ } else if (what == "end_bone_name") {
+ r_ret = get_end_bone_name(which);
+ } else if (what == "end_bone") {
+ String opt = path.get_slicec('/', 3);
+ if (opt.is_empty()) {
+ r_ret = get_end_bone(which);
+ } else if (opt == "direction") {
+ r_ret = (int)get_end_bone_direction(which);
+ } else if (opt == "length") {
+ r_ret = get_end_bone_length(which);
+ } else {
+ return false;
+ }
+ } else if (what == "use_virtual_end") {
+ r_ret = is_using_virtual_end(which);
+ } else if (what == "extend_end_bone") {
+ r_ret = is_end_bone_extended(which);
+ } else {
+ return false;
+ }
+ }
+ return true;
+}
+
+void TwoBoneIK3D::_get_property_list(List *p_list) const {
+ String enum_hint;
+ Skeleton3D *skeleton = get_skeleton();
+ if (skeleton) {
+ enum_hint = skeleton->get_concatenated_bone_names();
+ }
+
+ LocalVector props;
+
+ for (uint32_t i = 0; i < settings.size(); i++) {
+ String path = "settings/" + itos(i) + "/";
+ props.push_back(PropertyInfo(Variant::NODE_PATH, path + "target_node"));
+ props.push_back(PropertyInfo(Variant::NODE_PATH, path + "pole_node"));
+ props.push_back(PropertyInfo(Variant::STRING, path + "root_bone_name", PROPERTY_HINT_ENUM_SUGGESTION, enum_hint));
+ props.push_back(PropertyInfo(Variant::INT, path + "root_bone", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_NO_EDITOR));
+ props.push_back(PropertyInfo(Variant::STRING, path + "middle_bone_name", PROPERTY_HINT_ENUM_SUGGESTION, enum_hint));
+ props.push_back(PropertyInfo(Variant::INT, path + "middle_bone", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_NO_EDITOR));
+ props.push_back(PropertyInfo(Variant::INT, path + "pole_direction", PROPERTY_HINT_ENUM, SkeletonModifier3D::get_hint_secondary_direction()));
+ props.push_back(PropertyInfo(Variant::VECTOR3, path + "pole_direction_vector"));
+ props.push_back(PropertyInfo(Variant::STRING, path + "end_bone_name", PROPERTY_HINT_ENUM_SUGGESTION, enum_hint));
+ props.push_back(PropertyInfo(Variant::INT, path + "end_bone", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_NO_EDITOR));
+ props.push_back(PropertyInfo(Variant::BOOL, path + "use_virtual_end"));
+ props.push_back(PropertyInfo(Variant::BOOL, path + "extend_end_bone"));
+ props.push_back(PropertyInfo(Variant::INT, path + "end_bone/direction", PROPERTY_HINT_ENUM, SkeletonModifier3D::get_hint_bone_direction()));
+ props.push_back(PropertyInfo(Variant::FLOAT, path + "end_bone/length", PROPERTY_HINT_RANGE, "0,1,0.001,or_greater,suffix:m"));
+ }
+
+ for (PropertyInfo &p : props) {
+ _validate_dynamic_prop(p);
+ p_list->push_back(p);
+ }
+}
+
+void TwoBoneIK3D::_validate_dynamic_prop(PropertyInfo &p_property) const {
+ PackedStringArray split = p_property.name.split("/");
+ if (split.size() > 2 && split[0] == "settings") {
+ int which = split[1].to_int();
+
+ bool force_hide = false;
+ if ((split[2] == "end_bone" || split[2] == "end_bone_name") && split.size() == 3 && is_using_virtual_end(which)) {
+ p_property.usage = PROPERTY_USAGE_NONE;
+ }
+ if (split[2] == "use_virtual_end" && get_middle_bone(which) == -1) {
+ p_property.usage = PROPERTY_USAGE_NONE;
+ }
+ if (split[2] == "extend_end_bone") {
+ if (is_using_virtual_end(which)) {
+ p_property.usage = PROPERTY_USAGE_DEFAULT | PROPERTY_USAGE_READ_ONLY;
+ } else if (get_end_bone(which) == -1) {
+ p_property.usage = PROPERTY_USAGE_NONE;
+ force_hide = true;
+ }
+ }
+ if (force_hide || (split[2] == "end_bone" && !is_end_bone_extended(which) && split.size() > 3)) {
+ p_property.usage = PROPERTY_USAGE_NONE;
+ }
+
+ if (split[2] == "pole_direction_vector" && get_pole_direction(which) != SECONDARY_DIRECTION_CUSTOM) {
+ p_property.usage = PROPERTY_USAGE_NONE;
+ }
+ }
+}
+
+PackedStringArray TwoBoneIK3D::get_configuration_warnings() const {
+ PackedStringArray warnings = SkeletonModifier3D::get_configuration_warnings();
+ for (uint32_t i = 0; i < tb_settings.size(); i++) {
+ if (tb_settings[i]->target_node.is_empty()) {
+ warnings.push_back(RTR("Detecting settings with no target set! TwoBoneIK3D must have a target to work."));
+ break;
+ }
+ }
+ for (uint32_t i = 0; i < tb_settings.size(); i++) {
+ if (tb_settings[i]->target_node.is_empty()) {
+ warnings.push_back(RTR("Detecting settings with no pole target set! TwoBoneIK3D must have a pole target to work."));
+ break;
+ }
+ }
+ return warnings;
+}
+
+// Setting.
+
+void TwoBoneIK3D::set_root_bone_name(int p_index, const String &p_bone_name) {
+ ERR_FAIL_INDEX(p_index, (int)settings.size());
+ tb_settings[p_index]->root_bone.name = p_bone_name;
+ Skeleton3D *sk = get_skeleton();
+ if (sk) {
+ set_root_bone(p_index, sk->find_bone(tb_settings[p_index]->root_bone.name));
+ }
+}
+
+String TwoBoneIK3D::get_root_bone_name(int p_index) const {
+ ERR_FAIL_INDEX_V(p_index, (int)settings.size(), String());
+ return tb_settings[p_index]->root_bone.name;
+}
+
+void TwoBoneIK3D::set_root_bone(int p_index, int p_bone) {
+ ERR_FAIL_INDEX(p_index, (int)settings.size());
+ bool changed = tb_settings[p_index]->root_bone.bone != p_bone;
+ tb_settings[p_index]->root_bone.bone = p_bone;
+ Skeleton3D *sk = get_skeleton();
+ if (sk) {
+ if (tb_settings[p_index]->root_bone.bone <= -1 || tb_settings[p_index]->root_bone.bone >= sk->get_bone_count()) {
+ WARN_PRINT("Root bone index out of range!");
+ tb_settings[p_index]->root_bone.bone = -1;
+ } else {
+ tb_settings[p_index]->root_bone.name = sk->get_bone_name(tb_settings[p_index]->root_bone.bone);
+ }
+ }
+ if (changed) {
+ _update_joints(p_index);
+ }
+}
+
+int TwoBoneIK3D::get_root_bone(int p_index) const {
+ ERR_FAIL_INDEX_V(p_index, (int)settings.size(), -1);
+ return tb_settings[p_index]->root_bone.bone;
+}
+
+void TwoBoneIK3D::set_middle_bone_name(int p_index, const String &p_bone_name) {
+ ERR_FAIL_INDEX(p_index, (int)settings.size());
+ tb_settings[p_index]->middle_bone.name = p_bone_name;
+ Skeleton3D *sk = get_skeleton();
+ if (sk) {
+ set_middle_bone(p_index, sk->find_bone(tb_settings[p_index]->middle_bone.name));
+ }
+}
+
+String TwoBoneIK3D::get_middle_bone_name(int p_index) const {
+ ERR_FAIL_INDEX_V(p_index, (int)settings.size(), String());
+ return tb_settings[p_index]->middle_bone.name;
+}
+
+void TwoBoneIK3D::set_middle_bone(int p_index, int p_bone) {
+ ERR_FAIL_INDEX(p_index, (int)settings.size());
+ bool changed = tb_settings[p_index]->middle_bone.bone != p_bone;
+ tb_settings[p_index]->middle_bone.bone = p_bone;
+ Skeleton3D *sk = get_skeleton();
+ if (sk) {
+ if (tb_settings[p_index]->middle_bone.bone <= -1 || tb_settings[p_index]->middle_bone.bone >= sk->get_bone_count()) {
+ WARN_PRINT("Middle bone index out of range!");
+ tb_settings[p_index]->middle_bone.bone = -1;
+ tb_settings[p_index]->use_virtual_end = false; // To sync inspector.
+ } else {
+ tb_settings[p_index]->middle_bone.name = sk->get_bone_name(tb_settings[p_index]->middle_bone.bone);
+ }
+ }
+ if (changed) {
+ _update_joints(p_index);
+ }
+ notify_property_list_changed();
+}
+
+int TwoBoneIK3D::get_middle_bone(int p_index) const {
+ ERR_FAIL_INDEX_V(p_index, (int)settings.size(), -1);
+ return tb_settings[p_index]->middle_bone.bone;
+}
+
+void TwoBoneIK3D::set_end_bone_name(int p_index, const String &p_bone_name) {
+ ERR_FAIL_INDEX(p_index, (int)settings.size());
+ tb_settings[p_index]->end_bone.name = p_bone_name;
+ Skeleton3D *sk = get_skeleton();
+ if (sk) {
+ set_end_bone(p_index, sk->find_bone(tb_settings[p_index]->end_bone.name));
+ }
+}
+
+String TwoBoneIK3D::get_end_bone_name(int p_index) const {
+ ERR_FAIL_INDEX_V(p_index, (int)settings.size(), String());
+ return tb_settings[p_index]->end_bone.name;
+}
+
+void TwoBoneIK3D::set_end_bone(int p_index, int p_bone) {
+ ERR_FAIL_INDEX(p_index, (int)settings.size());
+ bool changed = tb_settings[p_index]->end_bone.bone != p_bone;
+ tb_settings[p_index]->end_bone.bone = p_bone;
+ Skeleton3D *sk = get_skeleton();
+ if (sk) {
+ if (tb_settings[p_index]->end_bone.bone <= -1 || tb_settings[p_index]->end_bone.bone >= sk->get_bone_count()) {
+ WARN_PRINT("End bone index out of range!");
+ tb_settings[p_index]->end_bone.bone = -1;
+ } else {
+ tb_settings[p_index]->end_bone.name = sk->get_bone_name(tb_settings[p_index]->end_bone.bone);
+ }
+ }
+ if (changed) {
+ _update_joints(p_index);
+ }
+ notify_property_list_changed();
+}
+
+int TwoBoneIK3D::get_end_bone(int p_index) const {
+ ERR_FAIL_INDEX_V(p_index, (int)settings.size(), -1);
+ return tb_settings[p_index]->get_end_bone();
+}
+
+void TwoBoneIK3D::set_use_virtual_end(int p_index, bool p_enabled) {
+ ERR_FAIL_INDEX(p_index, (int)settings.size());
+ bool changed = tb_settings[p_index]->use_virtual_end != p_enabled;
+ tb_settings[p_index]->use_virtual_end = p_enabled;
+ if (p_enabled) {
+ // To sync inspector.
+ tb_settings[p_index]->extend_end_bone = true;
+ }
+ tb_settings[p_index]->simulation_dirty = true;
+ if (changed) {
+ _update_joints(p_index);
+ }
+ notify_property_list_changed();
+}
+
+bool TwoBoneIK3D::is_using_virtual_end(int p_index) const {
+ ERR_FAIL_INDEX_V(p_index, (int)settings.size(), false);
+ return tb_settings[p_index]->use_virtual_end;
+}
+
+void TwoBoneIK3D::set_extend_end_bone(int p_index, bool p_enabled) {
+ ERR_FAIL_INDEX(p_index, (int)settings.size());
+ tb_settings[p_index]->extend_end_bone = p_enabled;
+ tb_settings[p_index]->simulation_dirty = true;
+ Skeleton3D *sk = get_skeleton();
+ if (sk) {
+ _validate_pole_direction(sk, p_index);
+ }
+ notify_property_list_changed();
+#ifdef TOOLS_ENABLED
+ update_gizmos();
+#endif // TOOLS_ENABLED
+}
+
+bool TwoBoneIK3D::is_end_bone_extended(int p_index) const {
+ ERR_FAIL_INDEX_V(p_index, (int)settings.size(), false);
+ return tb_settings[p_index]->extend_end_bone;
+}
+
+void TwoBoneIK3D::set_end_bone_direction(int p_index, BoneDirection p_bone_direction) {
+ ERR_FAIL_INDEX(p_index, (int)settings.size());
+ tb_settings[p_index]->end_bone_direction = p_bone_direction;
+ tb_settings[p_index]->simulation_dirty = true;
+ Skeleton3D *sk = get_skeleton();
+ if (sk) {
+ _validate_pole_direction(sk, p_index);
+ }
+#ifdef TOOLS_ENABLED
+ update_gizmos();
+#endif // TOOLS_ENABLED
+}
+
+SkeletonModifier3D::BoneDirection TwoBoneIK3D::get_end_bone_direction(int p_index) const {
+ ERR_FAIL_INDEX_V(p_index, (int)settings.size(), BONE_DIRECTION_FROM_PARENT);
+ return tb_settings[p_index]->end_bone_direction;
+}
+
+void TwoBoneIK3D::set_end_bone_length(int p_index, float p_length) {
+ ERR_FAIL_INDEX(p_index, (int)settings.size());
+ tb_settings[p_index]->end_bone_length = p_length;
+ tb_settings[p_index]->simulation_dirty = true;
+#ifdef TOOLS_ENABLED
+ update_gizmos();
+#endif // TOOLS_ENABLED
+}
+
+float TwoBoneIK3D::get_end_bone_length(int p_index) const {
+ ERR_FAIL_INDEX_V(p_index, (int)settings.size(), 0);
+ return tb_settings[p_index]->end_bone_length;
+}
+
+void TwoBoneIK3D::set_target_node(int p_index, const NodePath &p_node_path) {
+ ERR_FAIL_INDEX(p_index, (int)settings.size());
+ tb_settings[p_index]->target_node = p_node_path;
+ update_configuration_warnings();
+}
+
+NodePath TwoBoneIK3D::get_target_node(int p_index) const {
+ ERR_FAIL_INDEX_V(p_index, (int)settings.size(), NodePath());
+ return tb_settings[p_index]->target_node;
+}
+
+void TwoBoneIK3D::set_pole_node(int p_index, const NodePath &p_node_path) {
+ ERR_FAIL_INDEX(p_index, (int)settings.size());
+ tb_settings[p_index]->pole_node = p_node_path;
+ update_configuration_warnings();
+}
+
+NodePath TwoBoneIK3D::get_pole_node(int p_index) const {
+ ERR_FAIL_INDEX_V(p_index, (int)settings.size(), NodePath());
+ return tb_settings[p_index]->pole_node;
+}
+
+void TwoBoneIK3D::set_pole_direction(int p_index, SecondaryDirection p_direction) {
+ ERR_FAIL_INDEX(p_index, (int)settings.size());
+ tb_settings[p_index]->pole_direction = p_direction;
+ tb_settings[p_index]->simulation_dirty = true;
+ Skeleton3D *sk = get_skeleton();
+ if (sk) {
+ _validate_pole_direction(sk, p_index);
+ }
+ notify_property_list_changed();
+#ifdef TOOLS_ENABLED
+ update_gizmos();
+#endif // TOOLS_ENABLED
+}
+
+SkeletonModifier3D::SecondaryDirection TwoBoneIK3D::get_pole_direction(int p_index) const {
+ ERR_FAIL_INDEX_V(p_index, (int)settings.size(), SECONDARY_DIRECTION_NONE);
+ return tb_settings[p_index]->pole_direction;
+}
+
+void TwoBoneIK3D::set_pole_direction_vector(int p_index, const Vector3 &p_vector) {
+ ERR_FAIL_INDEX(p_index, (int)settings.size());
+ if (tb_settings[p_index]->pole_direction != SECONDARY_DIRECTION_CUSTOM) {
+ return;
+ }
+ tb_settings[p_index]->pole_direction_vector = p_vector;
+ tb_settings[p_index]->simulation_dirty = true;
+ Skeleton3D *sk = get_skeleton();
+ if (sk) {
+ _validate_pole_direction(sk, p_index);
+ }
+#ifdef TOOLS_ENABLED
+ update_gizmos();
+#endif // TOOLS_ENABLED
+}
+
+Vector3 TwoBoneIK3D::get_pole_direction_vector(int p_index) const {
+ ERR_FAIL_INDEX_V(p_index, (int)settings.size(), Vector3());
+ return tb_settings[p_index]->get_pole_direction_vector();
+}
+
+bool TwoBoneIK3D::is_valid(int p_index) const {
+ ERR_FAIL_INDEX_V(p_index, (int)settings.size(), false);
+ return tb_settings[p_index]->root_bone.bone != -1 && tb_settings[p_index]->middle_bone.bone != -1 && tb_settings[p_index]->is_end_valid();
+}
+
+void TwoBoneIK3D::_bind_methods() {
+ // Setting.
+ ClassDB::bind_method(D_METHOD("set_target_node", "index", "target_node"), &TwoBoneIK3D::set_target_node);
+ ClassDB::bind_method(D_METHOD("get_target_node", "index"), &TwoBoneIK3D::get_target_node);
+ ClassDB::bind_method(D_METHOD("set_pole_node", "index", "pole_node"), &TwoBoneIK3D::set_pole_node);
+ ClassDB::bind_method(D_METHOD("get_pole_node", "index"), &TwoBoneIK3D::get_pole_node);
+
+ ClassDB::bind_method(D_METHOD("set_root_bone_name", "index", "bone_name"), &TwoBoneIK3D::set_root_bone_name);
+ ClassDB::bind_method(D_METHOD("get_root_bone_name", "index"), &TwoBoneIK3D::get_root_bone_name);
+ ClassDB::bind_method(D_METHOD("set_root_bone", "index", "bone"), &TwoBoneIK3D::set_root_bone);
+ ClassDB::bind_method(D_METHOD("get_root_bone", "index"), &TwoBoneIK3D::get_root_bone);
+
+ ClassDB::bind_method(D_METHOD("set_middle_bone_name", "index", "bone_name"), &TwoBoneIK3D::set_middle_bone_name);
+ ClassDB::bind_method(D_METHOD("get_middle_bone_name", "index"), &TwoBoneIK3D::get_middle_bone_name);
+ ClassDB::bind_method(D_METHOD("set_middle_bone", "index", "bone"), &TwoBoneIK3D::set_middle_bone);
+ ClassDB::bind_method(D_METHOD("get_middle_bone", "index"), &TwoBoneIK3D::get_middle_bone);
+
+ ClassDB::bind_method(D_METHOD("set_pole_direction", "index", "direction"), &TwoBoneIK3D::set_pole_direction);
+ ClassDB::bind_method(D_METHOD("get_pole_direction", "index"), &TwoBoneIK3D::get_pole_direction);
+ ClassDB::bind_method(D_METHOD("set_pole_direction_vector", "index", "vector"), &TwoBoneIK3D::set_pole_direction_vector);
+ ClassDB::bind_method(D_METHOD("get_pole_direction_vector", "index"), &TwoBoneIK3D::get_pole_direction_vector);
+
+ ClassDB::bind_method(D_METHOD("set_end_bone_name", "index", "bone_name"), &TwoBoneIK3D::set_end_bone_name);
+ ClassDB::bind_method(D_METHOD("get_end_bone_name", "index"), &TwoBoneIK3D::get_end_bone_name);
+ ClassDB::bind_method(D_METHOD("set_end_bone", "index", "bone"), &TwoBoneIK3D::set_end_bone);
+ ClassDB::bind_method(D_METHOD("get_end_bone", "index"), &TwoBoneIK3D::get_end_bone);
+
+ ClassDB::bind_method(D_METHOD("set_use_virtual_end", "index", "enabled"), &TwoBoneIK3D::set_use_virtual_end);
+ ClassDB::bind_method(D_METHOD("is_using_virtual_end", "index"), &TwoBoneIK3D::is_using_virtual_end);
+ ClassDB::bind_method(D_METHOD("set_extend_end_bone", "index", "enabled"), &TwoBoneIK3D::set_extend_end_bone);
+ ClassDB::bind_method(D_METHOD("is_end_bone_extended", "index"), &TwoBoneIK3D::is_end_bone_extended);
+ ClassDB::bind_method(D_METHOD("set_end_bone_direction", "index", "bone_direction"), &TwoBoneIK3D::set_end_bone_direction);
+ ClassDB::bind_method(D_METHOD("get_end_bone_direction", "index"), &TwoBoneIK3D::get_end_bone_direction);
+ ClassDB::bind_method(D_METHOD("set_end_bone_length", "index", "length"), &TwoBoneIK3D::set_end_bone_length);
+ ClassDB::bind_method(D_METHOD("get_end_bone_length", "index"), &TwoBoneIK3D::get_end_bone_length);
+
+ ADD_ARRAY_COUNT("Settings", "setting_count", "set_setting_count", "get_setting_count", "settings/");
+}
+
+void TwoBoneIK3D::_validate_bone_names() {
+ for (uint32_t i = 0; i < settings.size(); i++) {
+ // Prior bone name.
+ if (!tb_settings[i]->root_bone.name.is_empty()) {
+ set_root_bone_name(i, tb_settings[i]->root_bone.name);
+ } else if (tb_settings[i]->root_bone.bone != -1) {
+ set_root_bone(i, tb_settings[i]->root_bone.bone);
+ }
+ // Prior bone name.
+ if (!tb_settings[i]->middle_bone.name.is_empty()) {
+ set_middle_bone_name(i, tb_settings[i]->middle_bone.name);
+ } else if (tb_settings[i]->middle_bone.bone != -1) {
+ set_middle_bone(i, tb_settings[i]->middle_bone.bone);
+ }
+ // Prior bone name.
+ if (!tb_settings[i]->end_bone.name.is_empty()) {
+ set_end_bone_name(i, tb_settings[i]->end_bone.name);
+ } else if (tb_settings[i]->end_bone.bone != -1) {
+ set_end_bone(i, tb_settings[i]->end_bone.bone);
+ }
+ }
+}
+
+void TwoBoneIK3D::_validate_pole_directions(Skeleton3D *p_skeleton) const {
+ for (uint32_t i = 0; i < settings.size(); i++) {
+ _validate_pole_direction(p_skeleton, i);
+ }
+}
+
+void TwoBoneIK3D::_validate_pole_direction(Skeleton3D *p_skeleton, int p_index) const {
+ TwoBoneIK3DSetting *setting = tb_settings[p_index];
+ SecondaryDirection dir = setting->pole_direction;
+ if (!is_valid(p_index) || dir == SECONDARY_DIRECTION_NONE) {
+ return;
+ }
+ Vector3 kv = get_pole_direction_vector(p_index).normalized();
+ Vector3 fwd;
+
+ // End bone.
+ int valid_end_bone = setting->get_end_bone();
+ Vector3 axis = get_bone_axis(valid_end_bone, setting->end_bone_direction);
+ Vector3 global_rest_origin;
+ if (setting->extend_end_bone && setting->end_bone_length > 0 && !axis.is_zero_approx()) {
+ global_rest_origin = p_skeleton->get_bone_global_rest(valid_end_bone).xform(axis * setting->end_bone_length);
+ } else {
+ // Shouldn't be using virtual end.
+ global_rest_origin = p_skeleton->get_bone_global_rest(valid_end_bone).origin;
+ }
+
+ // Middle bone.
+ axis = global_rest_origin - p_skeleton->get_bone_global_rest(setting->middle_bone.bone).origin;
+ if (!axis.is_zero_approx()) {
+ fwd = p_skeleton->get_bone_global_rest(setting->middle_bone.bone).basis.get_rotation_quaternion().xform_inv(axis).normalized();
+ } else {
+ return;
+ }
+
+ if (Math::is_equal_approx(Math::abs(kv.dot(fwd)), 1)) {
+ WARN_PRINT_ED("Setting: " + itos(p_index) + ": Pole direction and forward vector are colinear. This is not advised as it may cause unwanted rotation.");
+ }
+}
+
+void TwoBoneIK3D::_make_all_joints_dirty() {
+ for (uint32_t i = 0; i < settings.size(); i++) {
+ _update_joints(i);
+ }
+}
+
+void TwoBoneIK3D::_init_joints(Skeleton3D *p_skeleton, int p_index) {
+ TwoBoneIK3DSetting *setting = tb_settings[p_index];
+ cached_space = p_skeleton->get_global_transform_interpolated();
+ if (!setting->simulation_dirty) {
+ return;
+ }
+ setting->root_pos = Vector3();
+ setting->mid_pos = Vector3();
+ setting->end_pos = Vector3();
+ if (setting->root_joint_solver_info) {
+ memdelete(setting->root_joint_solver_info);
+ setting->root_joint_solver_info = nullptr;
+ }
+ if (setting->mid_joint_solver_info) {
+ memdelete(setting->mid_joint_solver_info);
+ setting->mid_joint_solver_info = nullptr;
+ }
+ if (setting->root_bone.bone == -1 || setting->middle_bone.bone == -1 || !setting->is_end_valid()) {
+ return;
+ }
+
+ // End bone.
+ int valid_end_bone = setting->get_end_bone();
+ Vector3 axis = get_bone_axis(valid_end_bone, setting->end_bone_direction);
+ Vector3 global_rest_origin;
+ if (setting->extend_end_bone && setting->end_bone_length > 0 && !axis.is_zero_approx()) {
+ setting->end_pos = p_skeleton->get_bone_global_pose(valid_end_bone).xform(axis * setting->end_bone_length);
+ global_rest_origin = p_skeleton->get_bone_global_rest(valid_end_bone).xform(axis * setting->end_bone_length);
+ } else {
+ // Shouldn't be using virtual end.
+ setting->end_pos = p_skeleton->get_bone_global_pose(valid_end_bone).origin;
+ global_rest_origin = p_skeleton->get_bone_global_rest(valid_end_bone).origin;
+ }
+
+ // Middle bone.
+ axis = global_rest_origin - p_skeleton->get_bone_global_rest(setting->middle_bone.bone).origin;
+ global_rest_origin = p_skeleton->get_bone_global_rest(setting->middle_bone.bone).origin;
+ if (!axis.is_zero_approx()) {
+ setting->mid_pos = p_skeleton->get_bone_global_pose(setting->middle_bone.bone).origin;
+ setting->mid_joint_solver_info = memnew(IKModifier3DSolverInfo);
+ setting->mid_joint_solver_info->forward_vector = p_skeleton->get_bone_global_rest(setting->middle_bone.bone).basis.get_rotation_quaternion().xform_inv(axis).normalized();
+ setting->mid_joint_solver_info->length = axis.length();
+ } else {
+ return;
+ }
+
+ // Root bone.
+ axis = global_rest_origin - p_skeleton->get_bone_global_rest(setting->root_bone.bone).origin;
+ global_rest_origin = p_skeleton->get_bone_global_rest(setting->root_bone.bone).origin;
+ if (!axis.is_zero_approx()) {
+ setting->root_pos = p_skeleton->get_bone_global_pose(setting->root_bone.bone).origin;
+ setting->root_joint_solver_info = memnew(IKModifier3DSolverInfo);
+ setting->root_joint_solver_info->forward_vector = p_skeleton->get_bone_global_rest(setting->root_bone.bone).basis.get_rotation_quaternion().xform_inv(axis).normalized();
+ setting->root_joint_solver_info->length = axis.length();
+ } else if (setting->mid_joint_solver_info) {
+ memdelete(setting->mid_joint_solver_info);
+ setting->mid_joint_solver_info = nullptr;
+ return;
+ }
+
+ setting->init_current_joint_rotations(p_skeleton);
+
+ double total_length = setting->root_joint_solver_info->length + setting->mid_joint_solver_info->length;
+ setting->cached_length_sq = total_length * total_length;
+
+ setting->simulation_dirty = false;
+}
+
+void TwoBoneIK3D::_update_joints(int p_index) {
+ tb_settings[p_index]->simulation_dirty = true;
+
+#ifdef TOOLS_ENABLED
+ update_gizmos(); // To clear invalid setting.
+#endif // TOOLS_ENABLED
+
+ Skeleton3D *sk = get_skeleton();
+ if (!sk || tb_settings[p_index]->root_bone.bone == -1 || tb_settings[p_index]->middle_bone.bone == -1 || !tb_settings[p_index]->is_end_valid()) {
+ return;
+ }
+
+ // Validation for middle bone.
+ int parent_bone = tb_settings[p_index]->root_bone.bone;
+ int current_bone = tb_settings[p_index]->middle_bone.bone;
+ bool valid = false;
+ while (current_bone >= 0) {
+ if (current_bone == parent_bone) {
+ valid = true;
+ break;
+ }
+ current_bone = sk->get_bone_parent(current_bone);
+ }
+ ERR_FAIL_COND_EDMSG(!valid, "The middle bone must be a child of the root bone.");
+
+ // Validation for end bone.
+ if (!tb_settings[p_index]->use_virtual_end) {
+ parent_bone = tb_settings[p_index]->middle_bone.bone;
+ current_bone = tb_settings[p_index]->end_bone.bone;
+ valid = false;
+ while (current_bone >= 0) {
+ if (current_bone == parent_bone) {
+ valid = true;
+ break;
+ }
+ current_bone = sk->get_bone_parent(current_bone);
+ }
+ ERR_FAIL_COND_EDMSG(!valid, "The end bone must be a child of the middle bone.");
+ }
+
+ if (sk) {
+ _validate_pole_directions(sk);
+ }
+
+#ifdef TOOLS_ENABLED
+ update_gizmos();
+#endif // TOOLS_ENABLED
+}
+
+void TwoBoneIK3D::_make_simulation_dirty(int p_index) {
+ TwoBoneIK3DSetting *setting = tb_settings[p_index];
+ if (!setting) {
+ return;
+ }
+ setting->simulation_dirty = true;
+}
+
+void TwoBoneIK3D::_process_ik(Skeleton3D *p_skeleton, double p_delta) {
+ for (uint32_t i = 0; i < settings.size(); i++) {
+ _init_joints(p_skeleton, i);
+ Node3D *target = Object::cast_to(get_node_or_null(tb_settings[i]->target_node));
+ Node3D *pole = Object::cast_to(get_node_or_null(tb_settings[i]->pole_node));
+ if (!target || !pole || !tb_settings[i]->is_valid()) {
+ continue; // Abort.
+ }
+ Vector3 destination = cached_space.affine_inverse().xform(target->get_global_transform_interpolated().origin);
+ Vector3 pole_destination = cached_space.affine_inverse().xform(pole->get_global_transform_interpolated().origin);
+ tb_settings[i]->cache_current_joint_rotations(p_skeleton, pole_destination); // Iterate over first to detect parent (outside of the chain) bone pose changes.
+ _process_joints(p_delta, p_skeleton, tb_settings[i], destination, pole_destination);
+ }
+}
+
+void TwoBoneIK3D::_process_joints(double p_delta, Skeleton3D *p_skeleton, TwoBoneIK3DSetting *p_setting, const Vector3 &p_destination, const Vector3 &p_pole_destination) {
+ // Solve the IK for this iteration.
+ Vector3 destination = p_destination;
+
+ // Make vector from root to destination.
+ p_setting->root_pos = p_skeleton->get_bone_global_pose(p_setting->root_bone.bone).origin; // New root position.
+ Vector3 root_to_destination = destination - p_setting->root_pos;
+ if (root_to_destination.is_zero_approx()) {
+ return; // Abort.
+ }
+
+ double rd_len_sq = root_to_destination.length_squared();
+ // Compare the distance to the target with the length of the bones.
+ if (rd_len_sq >= p_setting->cached_length_sq) {
+ // Result is straight.
+ Vector3 rd_nrm = root_to_destination.normalized();
+ p_setting->mid_pos = p_setting->root_pos + rd_nrm * p_setting->root_joint_solver_info->length;
+ p_setting->end_pos = p_setting->mid_pos + rd_nrm * p_setting->mid_joint_solver_info->length;
+ } else {
+ // Check if the target can be reached by subtracting the lengths of the bones.
+ // If not, push out target to normal of the root bone sphere.
+ double sub = p_setting->root_joint_solver_info->length - p_setting->mid_joint_solver_info->length;
+ if (rd_len_sq < sub * sub) {
+ Vector3 push_nrm = (destination - p_setting->root_pos).normalized();
+ destination = p_setting->root_pos + push_nrm * Math::abs(sub);
+ root_to_destination = destination - p_setting->root_pos;
+ }
+
+ // End is snapped to the target.
+ p_setting->end_pos = destination;
+
+ // Result is bent, determine the mid position to respect the pole target.
+ // Mid-position should be a point of intersection of two circles.
+ double l_chain = root_to_destination.length();
+ Vector3 u = root_to_destination.normalized();
+ Vector3 pole_vec = get_projected_normal(p_setting->root_pos, p_setting->end_pos, p_pole_destination);
+
+ // Circle1: center is the root, radius is the length of the root bone.
+ double r_root = p_setting->root_joint_solver_info->length;
+ // Circle2: center is the target, radius is the length of the middle bone.
+ double r_mid = p_setting->mid_joint_solver_info->length;
+
+ double a = (l_chain * l_chain + r_root * r_root - r_mid * r_mid) / (2.0 * l_chain);
+ double h2 = r_root * r_root - a * a;
+ if (h2 < 0) {
+ h2 = 0;
+ }
+ double h = Math::sqrt(h2);
+
+ Vector3 det_plus = (p_setting->root_pos + u * a) + pole_vec * h;
+ Vector3 det_minus = (p_setting->root_pos + u * a) - pole_vec * h;
+
+ // Pick the intersection that is closest to the pole target.
+ p_setting->mid_pos = p_pole_destination.distance_squared_to(det_plus) < p_pole_destination.distance_squared_to(det_minus) ? det_plus : det_minus;
+ }
+
+ // Update virtual bone rest/poses.
+ p_setting->cache_current_vectors(p_skeleton);
+ p_setting->cache_current_joint_rotations(p_skeleton, p_pole_destination);
+
+ // Apply the virtual bone rest/poses to the actual bones.
+ p_skeleton->set_bone_pose_rotation(p_setting->root_bone.bone, p_setting->root_joint_solver_info->current_lpose);
+ // Mid joint pose is relative to the root joint pose for the case root-mid or mid-end have more than 1 joints.
+ p_skeleton->set_bone_pose_rotation(p_setting->middle_bone.bone, get_local_pose_rotation(p_skeleton, p_setting->middle_bone.bone, p_setting->mid_joint_solver_info->current_gpose));
+}
+
+TwoBoneIK3D::~TwoBoneIK3D() {
+ clear_settings();
+}
diff --git a/scene/3d/two_bone_ik_3d.h b/scene/3d/two_bone_ik_3d.h
new file mode 100644
index 00000000000..e93a5637870
--- /dev/null
+++ b/scene/3d/two_bone_ik_3d.h
@@ -0,0 +1,311 @@
+/**************************************************************************/
+/* two_bone_ik_3d.h */
+/**************************************************************************/
+/* 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. */
+/**************************************************************************/
+
+#pragma once
+
+#include "scene/3d/ik_modifier_3d.h"
+
+class TwoBoneIK3D : public IKModifier3D {
+ GDCLASS(TwoBoneIK3D, IKModifier3D);
+
+public:
+ struct TwoBoneIK3DSetting : public IKModifier3DSetting {
+ bool joints_dirty = false;
+
+ BoneJoint root_bone;
+ BoneJoint middle_bone;
+ BoneJoint end_bone;
+
+ // To make virtual end joint.
+ bool use_virtual_end = false;
+ bool extend_end_bone = false;
+ BoneDirection end_bone_direction = BONE_DIRECTION_FROM_PARENT;
+ float end_bone_length = 0.0;
+
+ NodePath pole_node;
+ SecondaryDirection pole_direction = SECONDARY_DIRECTION_NONE; // Sort to pole target plane.
+ Vector3 pole_direction_vector; // Custom vector.
+ NodePath target_node;
+
+ IKModifier3DSolverInfo *root_joint_solver_info = nullptr;
+ IKModifier3DSolverInfo *mid_joint_solver_info = nullptr;
+ Vector3 root_pos;
+ Vector3 mid_pos;
+ Vector3 end_pos;
+
+ // To process.
+ bool simulation_dirty = true;
+ double cached_length_sq = 0.0;
+
+ bool is_valid() const {
+ return root_joint_solver_info && mid_joint_solver_info;
+ }
+ bool is_end_valid() const {
+ return (!use_virtual_end && end_bone.bone != -1) || (use_virtual_end && !Math::is_zero_approx(end_bone_length));
+ }
+ int get_end_bone() const {
+ return use_virtual_end ? middle_bone.bone : end_bone.bone; // Hack, but useful for external class such as TwoBoneIK3DGizmoPlugin.
+ }
+
+ Vector3 get_pole_direction_vector() const {
+ Vector3 ret;
+ switch (pole_direction) {
+ case SECONDARY_DIRECTION_NONE:
+ ret = Vector3(0, 0, 0);
+ break;
+ case SECONDARY_DIRECTION_PLUS_X:
+ ret = Vector3(1, 0, 0);
+ break;
+ case SECONDARY_DIRECTION_MINUS_X:
+ ret = Vector3(-1, 0, 0);
+ break;
+ case SECONDARY_DIRECTION_PLUS_Y:
+ ret = Vector3(0, 1, 0);
+ break;
+ case SECONDARY_DIRECTION_MINUS_Y:
+ ret = Vector3(0, -1, 0);
+ break;
+ case SECONDARY_DIRECTION_PLUS_Z:
+ ret = Vector3(0, 0, 1);
+ break;
+ case SECONDARY_DIRECTION_MINUS_Z:
+ ret = Vector3(0, 0, -1);
+ break;
+ case SECONDARY_DIRECTION_CUSTOM:
+ ret = pole_direction_vector;
+ break;
+ }
+ return ret;
+ }
+
+ void cache_current_vectors(Skeleton3D *p_skeleton) {
+ if (!is_valid()) {
+ return;
+ }
+ root_joint_solver_info->current_vector = (mid_pos - root_pos).normalized();
+ mid_joint_solver_info->current_vector = (end_pos - mid_pos).normalized();
+ }
+
+ void init_current_joint_rotations(Skeleton3D *p_skeleton) {
+ if (!is_valid()) {
+ return;
+ }
+
+ Quaternion parent_gpose;
+ int parent = p_skeleton->get_bone_parent(root_bone.bone);
+ if (parent >= 0) {
+ parent_gpose = p_skeleton->get_bone_global_pose(parent).basis.get_rotation_quaternion();
+ }
+ root_joint_solver_info->current_lrest = p_skeleton->get_bone_pose(root_bone.bone).basis.get_rotation_quaternion();
+ root_joint_solver_info->current_grest = parent_gpose * root_joint_solver_info->current_lrest;
+ root_joint_solver_info->current_grest.normalize();
+ root_joint_solver_info->current_lpose = p_skeleton->get_bone_pose(root_bone.bone).basis.get_rotation_quaternion();
+ root_joint_solver_info->current_gpose = parent_gpose * root_joint_solver_info->current_lpose;
+ root_joint_solver_info->current_gpose.normalize();
+ parent_gpose = root_joint_solver_info->current_gpose;
+
+ // Mid joint pose is relative to the root joint pose.
+ mid_joint_solver_info->current_lrest = p_skeleton->get_bone_global_pose(root_bone.bone).basis.get_rotation_quaternion().inverse() * p_skeleton->get_bone_global_pose(middle_bone.bone).basis.get_rotation_quaternion();
+ mid_joint_solver_info->current_grest = parent_gpose * mid_joint_solver_info->current_lrest;
+ mid_joint_solver_info->current_grest.normalize();
+ mid_joint_solver_info->current_lpose = p_skeleton->get_bone_global_pose(root_bone.bone).basis.get_rotation_quaternion().inverse() * p_skeleton->get_bone_global_pose(middle_bone.bone).basis.get_rotation_quaternion();
+ mid_joint_solver_info->current_gpose = parent_gpose * mid_joint_solver_info->current_lpose;
+ mid_joint_solver_info->current_gpose.normalize();
+
+ cache_current_vectors(p_skeleton);
+ }
+
+ // Make rotation as bone pose from chain coordinates.
+ void cache_current_joint_rotations(Skeleton3D *p_skeleton, const Vector3 &p_pole_destination) {
+ if (!is_valid()) {
+ return;
+ }
+
+ Quaternion parent_gpose;
+ int parent = p_skeleton->get_bone_parent(root_bone.bone);
+ if (parent >= 0) {
+ parent_gpose = p_skeleton->get_bone_global_pose(parent).basis.get_rotation_quaternion();
+ }
+
+ root_joint_solver_info->current_lrest = p_skeleton->get_bone_pose(root_bone.bone).basis.get_rotation_quaternion();
+ root_joint_solver_info->current_grest = parent_gpose * root_joint_solver_info->current_lrest;
+ root_joint_solver_info->current_grest.normalize();
+
+ Vector3 from = root_joint_solver_info->forward_vector;
+ Vector3 to = root_joint_solver_info->current_grest.xform_inv(root_joint_solver_info->current_vector).normalized();
+ root_joint_solver_info->current_lpose = root_joint_solver_info->current_lrest * get_swing(Quaternion(from, to), from);
+
+ root_joint_solver_info->current_gpose = parent_gpose * root_joint_solver_info->current_lpose;
+ root_joint_solver_info->current_gpose.normalize();
+ Quaternion root_gpose = root_joint_solver_info->current_gpose;
+
+ // Mid joint pose is relative to the root joint pose for the case root-mid or mid-end have more than 1 joints.
+ mid_joint_solver_info->current_lrest = p_skeleton->get_bone_global_pose(root_bone.bone).basis.get_rotation_quaternion().inverse() * p_skeleton->get_bone_global_pose(middle_bone.bone).basis.get_rotation_quaternion();
+ mid_joint_solver_info->current_grest = root_gpose * mid_joint_solver_info->current_lrest;
+ mid_joint_solver_info->current_grest.normalize();
+
+ from = mid_joint_solver_info->forward_vector;
+ to = mid_joint_solver_info->current_grest.xform_inv(mid_joint_solver_info->current_vector).normalized();
+ mid_joint_solver_info->current_lpose = mid_joint_solver_info->current_lrest * get_swing(Quaternion(from, to), from);
+
+ mid_joint_solver_info->current_gpose = root_gpose * mid_joint_solver_info->current_lpose;
+ mid_joint_solver_info->current_gpose.normalize();
+
+ bool is_pole_defined = pole_direction != SECONDARY_DIRECTION_NONE && (pole_direction != SECONDARY_DIRECTION_CUSTOM || !pole_direction_vector.is_zero_approx());
+ // Fix roll to align pole vector to plane.
+ if (is_pole_defined) {
+ // Calc roll angles.
+ Quaternion root_roll_rot = Quaternion();
+ Quaternion mid_roll_rot = Quaternion();
+
+ // Make roll to align pole_vector onto plane with selecting the point nearer pole_destination.
+ Vector3 pole_dir = get_projected_normal(root_pos, end_pos, p_pole_destination);
+ if (pole_dir.is_zero_approx()) {
+ return;
+ }
+ Vector3 a = mid_joint_solver_info->current_vector.normalized(); // Global roll axis (mid forward in current pose).
+ Vector3 k = mid_joint_solver_info->current_gpose.xform(get_pole_direction_vector()).normalized(); // Global pole vector.
+ Vector3 n = pole_dir.cross((mid_pos - root_pos).normalized()).normalized(); // Global plane normal.
+
+ // Guard: degenerate cases (zero or already parallel)
+ if (a.is_zero_approx() || k.is_zero_approx() || n.is_zero_approx() || Math::is_zero_approx(n.dot(k))) {
+ return;
+ }
+ // c0 cosθ + c1 sinθ + c2 = 0
+ double c0 = n.dot(k - a * k.dot(a)); // n·(k⊥a)
+ double c1 = n.dot(a.cross(k)); // n·(a×k)
+ double c2 = n.dot(a) * k.dot(a); // (n·a)(k·a)
+ double r = Math::sqrt(c0 * c0 + c1 * c1);
+ double cos_arg = CLAMP(-c2 / r, -1.0, 1.0);
+ double phi = Math::atan2(c1, c0);
+ double acosv = Math::acos(cos_arg);
+
+ // Two candidate angles.
+ double t1 = phi + acosv;
+ double t2 = phi - acosv;
+ Quaternion q1(a, t1);
+ Quaternion q2(a, t2);
+
+ // Choose the one whose projected pole points closer to pole side.
+ Vector3 pole_proj = snap_vector_to_plane(n, pole_dir).normalized();
+ Vector3 k1p = snap_vector_to_plane(n, q1.xform(k)).normalized();
+ Vector3 k2p = snap_vector_to_plane(n, q2.xform(k)).normalized();
+ double s1 = pole_proj.is_zero_approx() ? Math::abs(t1) : k1p.dot(pole_proj);
+ double s2 = pole_proj.is_zero_approx() ? Math::abs(t2) : k2p.dot(pole_proj);
+
+ double t = s1 >= s2 ? t1 : t2;
+ root_roll_rot = Quaternion(root_joint_solver_info->forward_vector, t);
+ mid_roll_rot = Quaternion(mid_joint_solver_info->forward_vector, t);
+
+ root_joint_solver_info->current_lpose = root_joint_solver_info->current_lpose * root_roll_rot;
+ root_joint_solver_info->current_gpose = parent_gpose * root_joint_solver_info->current_lpose;
+ root_joint_solver_info->current_gpose.normalize();
+ root_gpose = root_joint_solver_info->current_gpose;
+
+ mid_joint_solver_info->current_lpose = root_roll_rot.inverse() * mid_joint_solver_info->current_lpose * mid_roll_rot;
+ mid_joint_solver_info->current_gpose = root_gpose * mid_joint_solver_info->current_lpose;
+ mid_joint_solver_info->current_gpose.normalize();
+ }
+ }
+ };
+
+protected:
+ LocalVector tb_settings;
+
+ bool _get(const StringName &p_path, Variant &r_ret) const;
+ bool _set(const StringName &p_path, const Variant &p_value);
+ void _get_property_list(List *p_list) const;
+ void _validate_dynamic_prop(PropertyInfo &p_property) const;
+
+ static void _bind_methods();
+
+ virtual void _validate_bone_names() override;
+ void _validate_pole_directions(Skeleton3D *p_skeleton) const;
+ void _validate_pole_direction(Skeleton3D *p_skeleton, int p_index) const;
+
+ virtual void _make_all_joints_dirty() override;
+ virtual void _init_joints(Skeleton3D *p_skeleton, int p_index) override;
+ virtual void _update_joints(int p_index) override;
+ virtual void _make_simulation_dirty(int p_index) override;
+
+ virtual void _process_ik(Skeleton3D *p_skeleton, double p_delta) override;
+ void _process_joints(double p_delta, Skeleton3D *p_skeleton, TwoBoneIK3DSetting *p_setting, const Vector3 &p_destination, const Vector3 &p_pole_destination);
+
+public:
+ virtual PackedStringArray get_configuration_warnings() const override;
+ virtual void set_setting_count(int p_count) override {
+ _set_setting_count(p_count);
+ tb_settings = _cast_settings();
+ }
+ virtual void clear_settings() override {
+ _set_setting_count(0);
+ tb_settings.clear();
+ }
+
+ // Setting.
+ void set_root_bone_name(int p_index, const String &p_bone_name);
+ String get_root_bone_name(int p_index) const;
+ void set_root_bone(int p_index, int p_bone);
+ int get_root_bone(int p_index) const;
+
+ void set_middle_bone_name(int p_index, const String &p_bone_name);
+ String get_middle_bone_name(int p_index) const;
+ void set_middle_bone(int p_index, int p_bone);
+ int get_middle_bone(int p_index) const;
+
+ void set_end_bone_name(int p_index, const String &p_bone_name);
+ String get_end_bone_name(int p_index) const;
+ void set_end_bone(int p_index, int p_bone);
+ int get_end_bone(int p_index) const;
+
+ void set_use_virtual_end(int p_index, bool p_enabled);
+ bool is_using_virtual_end(int p_index) const;
+ void set_extend_end_bone(int p_index, bool p_enabled);
+ bool is_end_bone_extended(int p_index) const;
+ void set_end_bone_direction(int p_index, BoneDirection p_bone_direction);
+ BoneDirection get_end_bone_direction(int p_index) const;
+ void set_end_bone_length(int p_index, float p_length);
+ float get_end_bone_length(int p_index) const;
+
+ void set_target_node(int p_index, const NodePath &p_target_node);
+ NodePath get_target_node(int p_index) const;
+
+ void set_pole_node(int p_index, const NodePath &p_pole_node);
+ NodePath get_pole_node(int p_index) const;
+
+ void set_pole_direction(int p_index, SecondaryDirection p_axis);
+ SecondaryDirection get_pole_direction(int p_index) const;
+ void set_pole_direction_vector(int p_index, const Vector3 &p_vector);
+ Vector3 get_pole_direction_vector(int p_index) const;
+
+ bool is_valid(int p_index) const; // Helper for editor and validation.
+
+ ~TwoBoneIK3D();
+};
diff --git a/scene/gui/control.h b/scene/gui/control.h
index f2fb1b13f94..943569af1b4 100644
--- a/scene/gui/control.h
+++ b/scene/gui/control.h
@@ -46,7 +46,7 @@ class Control : public CanvasItem {
#ifdef TOOLS_ENABLED
bool saving = false;
-#endif //TOOLS_ENABLED
+#endif // TOOLS_ENABLED
public:
static constexpr AncestralClass static_ancestral_class = AncestralClass::CONTROL;
diff --git a/scene/register_scene_types.cpp b/scene/register_scene_types.cpp
index 0ecd1d2c4a3..05505acc13f 100644
--- a/scene/register_scene_types.cpp
+++ b/scene/register_scene_types.cpp
@@ -221,14 +221,20 @@
#include "scene/3d/bone_attachment_3d.h"
#include "scene/3d/bone_constraint_3d.h"
#include "scene/3d/camera_3d.h"
+#include "scene/3d/ccd_ik_3d.h"
+#include "scene/3d/chain_ik_3d.h"
#include "scene/3d/convert_transform_modifier_3d.h"
#include "scene/3d/copy_transform_modifier_3d.h"
#include "scene/3d/cpu_particles_3d.h"
#include "scene/3d/decal.h"
+#include "scene/3d/fabr_ik_3d.h"
#include "scene/3d/fog_volume.h"
#include "scene/3d/gpu_particles_3d.h"
#include "scene/3d/gpu_particles_collision_3d.h"
+#include "scene/3d/ik_modifier_3d.h"
#include "scene/3d/importer_mesh_instance_3d.h"
+#include "scene/3d/iterate_ik_3d.h"
+#include "scene/3d/jacobian_ik_3d.h"
#include "scene/3d/label_3d.h"
#include "scene/3d/light_3d.h"
#include "scene/3d/lightmap_gi.h"
@@ -246,12 +252,14 @@
#include "scene/3d/retarget_modifier_3d.h"
#include "scene/3d/skeleton_3d.h"
#include "scene/3d/skeleton_modifier_3d.h"
+#include "scene/3d/spline_ik_3d.h"
#include "scene/3d/spring_bone_collision_3d.h"
#include "scene/3d/spring_bone_collision_capsule_3d.h"
#include "scene/3d/spring_bone_collision_plane_3d.h"
#include "scene/3d/spring_bone_collision_sphere_3d.h"
#include "scene/3d/spring_bone_simulator_3d.h"
#include "scene/3d/sprite_3d.h"
+#include "scene/3d/two_bone_ik_3d.h"
#include "scene/3d/visible_on_screen_notifier_3d.h"
#include "scene/3d/voxel_gi.h"
#include "scene/3d/world_environment.h"
@@ -345,6 +353,12 @@
#include "scene/resources/3d/convex_polygon_shape_3d.h"
#include "scene/resources/3d/cylinder_shape_3d.h"
#include "scene/resources/3d/height_map_shape_3d.h"
+#include "scene/resources/3d/importer_mesh.h"
+#include "scene/resources/3d/joint_limitation_3d.h"
+#include "scene/resources/3d/joint_limitation_cone_3d.h"
+#include "scene/resources/3d/mesh_library.h"
+#include "scene/resources/3d/navigation_mesh_source_geometry_data_3d.h"
+#include "scene/resources/3d/primitive_meshes.h"
#include "scene/resources/3d/separation_ray_shape_3d.h"
#include "scene/resources/3d/sphere_shape_3d.h"
#include "scene/resources/3d/world_boundary_shape_3d.h"
@@ -646,6 +660,8 @@ void register_scene_types() {
GDREGISTER_VIRTUAL_CLASS(SkeletonModifier3D);
GDREGISTER_CLASS(ModifierBoneTarget3D);
GDREGISTER_CLASS(RetargetModifier3D);
+ GDREGISTER_VIRTUAL_CLASS(JointLimitation3D);
+ GDREGISTER_CLASS(JointLimitationCone3D);
GDREGISTER_CLASS(SpringBoneSimulator3D);
GDREGISTER_VIRTUAL_CLASS(SpringBoneCollision3D);
GDREGISTER_CLASS(SpringBoneCollisionSphere3D);
@@ -655,6 +671,14 @@ void register_scene_types() {
GDREGISTER_CLASS(CopyTransformModifier3D);
GDREGISTER_CLASS(ConvertTransformModifier3D);
GDREGISTER_CLASS(AimModifier3D);
+ GDREGISTER_ABSTRACT_CLASS(IKModifier3D);
+ GDREGISTER_CLASS(TwoBoneIK3D);
+ GDREGISTER_ABSTRACT_CLASS(ChainIK3D);
+ GDREGISTER_CLASS(SplineIK3D);
+ GDREGISTER_ABSTRACT_CLASS(IterateIK3D);
+ GDREGISTER_CLASS(FABRIK3D);
+ GDREGISTER_CLASS(CCDIK3D);
+ GDREGISTER_CLASS(JacobianIK3D);
#ifndef XR_DISABLED
GDREGISTER_CLASS(XRCamera3D);
diff --git a/scene/resources/3d/SCsub b/scene/resources/3d/SCsub
index e8a11591428..65704e93ee0 100644
--- a/scene/resources/3d/SCsub
+++ b/scene/resources/3d/SCsub
@@ -5,6 +5,8 @@ Import("env")
env.add_source_files(env.scene_sources, "fog_material.cpp")
env.add_source_files(env.scene_sources, "importer_mesh.cpp")
+env.add_source_files(env.scene_sources, "joint_limitation_3d.cpp")
+env.add_source_files(env.scene_sources, "joint_limitation_cone_3d.cpp")
env.add_source_files(env.scene_sources, "mesh_library.cpp")
env.add_source_files(env.scene_sources, "primitive_meshes.cpp")
env.add_source_files(env.scene_sources, "skin.cpp")
diff --git a/scene/resources/3d/joint_limitation_3d.cpp b/scene/resources/3d/joint_limitation_3d.cpp
new file mode 100644
index 00000000000..d6bab4dbf32
--- /dev/null
+++ b/scene/resources/3d/joint_limitation_3d.cpp
@@ -0,0 +1,65 @@
+/**************************************************************************/
+/* joint_limitation_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 "joint_limitation_3d.h"
+
+#include "scene/3d/skeleton_modifier_3d.h"
+
+Quaternion JointLimitation3D::make_space(const Vector3 &p_local_forward_vector, const Vector3 &p_local_right_vector, const Quaternion &p_rotation_offset) const {
+ const double ALMOST_ONE = 1.0 - CMP_EPSILON;
+ // The default is to interpret the forward vector as the +Y axis.
+ Vector3 axis_y = p_local_forward_vector.normalized();
+ Vector3 axis_x = p_local_right_vector.normalized();
+ if (axis_x.is_zero_approx() || Math::abs(axis_x.dot(axis_y)) > ALMOST_ONE) {
+ return (Quaternion(Vector3(0, 1, 0), axis_y) * p_rotation_offset.normalized()).normalized();
+ }
+ // Prior X axis.
+ Vector3 axis_z = axis_x.cross(axis_y);
+ axis_z.normalize();
+ axis_x = axis_y.cross(axis_z);
+ axis_x.normalize();
+ return (Basis(axis_x, axis_y, axis_z).get_rotation_quaternion() * p_rotation_offset.normalized()).normalized();
+}
+
+Vector3 JointLimitation3D::_solve(const Vector3 &p_direction) const {
+ return p_direction;
+}
+
+Vector3 JointLimitation3D::solve(const Vector3 &p_local_forward_vector, const Vector3 &p_local_right_vector, const Quaternion &p_rotation_offset, const Vector3 &p_local_current_vector) const {
+ Quaternion space = make_space(p_local_forward_vector, p_local_right_vector, p_rotation_offset);
+ Vector3 dir = p_local_current_vector.normalized();
+ return space.xform(_solve(space.xform_inv(dir)));
+}
+
+#ifdef TOOLS_ENABLED
+void JointLimitation3D::draw_shape(Ref &p_surface_tool, const Transform3D &p_transform, float p_bone_length, const Color &p_color) const {
+ //
+}
+#endif // TOOLS_ENABLED
diff --git a/scene/resources/3d/joint_limitation_3d.h b/scene/resources/3d/joint_limitation_3d.h
new file mode 100644
index 00000000000..71589a145d6
--- /dev/null
+++ b/scene/resources/3d/joint_limitation_3d.h
@@ -0,0 +1,55 @@
+/**************************************************************************/
+/* joint_limitation_3d.h */
+/**************************************************************************/
+/* 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. */
+/**************************************************************************/
+
+#pragma once
+
+#include "core/io/resource.h"
+
+#ifdef TOOLS_ENABLED
+#include "scene/resources/surface_tool.h"
+#endif // TOOLS_ENABLED
+
+class JointLimitation3D : public Resource {
+ GDCLASS(JointLimitation3D, Resource);
+
+protected:
+ // Directions are normalized vector from Vector(0, 0, 0). Space is defined by _make_space(), must return normalized vector.
+ virtual Vector3 _solve(const Vector3 &p_direction) const;
+
+public:
+ // Define temporary space based on rest and forward vector.
+ virtual Quaternion make_space(const Vector3 &p_local_forward_vector, const Vector3 &p_local_right_vector, const Quaternion &p_rotation_offset) const;
+
+ Vector3 solve(const Vector3 &p_local_forward_vector, const Vector3 &p_local_right_vector, const Quaternion &p_rotation_offset, const Vector3 &p_local_current_vector) const;
+
+#ifdef TOOLS_ENABLED
+ virtual void draw_shape(Ref &p_surface_tool, const Transform3D &p_transform, float p_bone_length, const Color &p_color) const; // For drawing gizmo.
+#endif // TOOLS_ENABLED
+};
diff --git a/scene/resources/3d/joint_limitation_cone_3d.cpp b/scene/resources/3d/joint_limitation_cone_3d.cpp
new file mode 100644
index 00000000000..bfbdd40ecf1
--- /dev/null
+++ b/scene/resources/3d/joint_limitation_cone_3d.cpp
@@ -0,0 +1,182 @@
+/**************************************************************************/
+/* joint_limitation_cone_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 "joint_limitation_cone_3d.h"
+
+void JointLimitationCone3D::set_radius_range(real_t p_radius_range) {
+ radius_range = p_radius_range;
+ emit_changed();
+}
+
+real_t JointLimitationCone3D::get_radius_range() const {
+ return radius_range;
+}
+
+void JointLimitationCone3D::_bind_methods() {
+ ClassDB::bind_method(D_METHOD("set_radius_range", "radius_range"), &JointLimitationCone3D::set_radius_range);
+ ClassDB::bind_method(D_METHOD("get_radius_range"), &JointLimitationCone3D::get_radius_range);
+
+ ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "radius_range", PROPERTY_HINT_RANGE, "0,1,0.01"), "set_radius_range", "get_radius_range");
+}
+
+Vector3 JointLimitationCone3D::_solve(const Vector3 &p_direction) const {
+ // Assume the central (forward of the cone) axis is the +Y.
+ // This is based on the coordinate system set by JointLimitation3D::_make_space().
+ Vector3 center_axis = Vector3(0, 1, 0);
+
+ // Apply the limitation if the angle exceeds radius_range * PI.
+ real_t angle = p_direction.angle_to(center_axis);
+ real_t max_angle = radius_range * Math::PI;
+
+ if (angle <= max_angle) {
+ // If within the limitation range, return the new direction as is.
+ return p_direction;
+ }
+
+ // If outside the limitation range, calculate the closest direction within the range.
+ // Define a plane using the central axis and the new direction vector.
+ Vector3 plane_normal;
+
+ // Special handling for when the new direction vector is completely opposite to the central axis.
+ if (Math::is_equal_approx((double)angle, Math::PI)) {
+ // Select an arbitrary perpendicular axis
+ plane_normal = center_axis.get_any_perpendicular();
+ } else {
+ plane_normal = center_axis.cross(p_direction).normalized();
+ }
+
+ // Calculate a vector rotated by the maximum angle from the central axis on the plane.
+ Quaternion rotation = Quaternion(plane_normal, max_angle);
+ Vector3 limited_dir = rotation.xform(center_axis);
+
+ // Return the vector within the limitation range that is closest to p_direction.
+ // This preserves the directionality of p_direction as much as possible.
+ Vector3 projection = p_direction - center_axis * p_direction.dot(center_axis);
+ if (projection.length_squared() > CMP_EPSILON) {
+ Vector3 side_dir = projection.normalized();
+ Quaternion side_rotation = Quaternion(center_axis.cross(side_dir).normalized(), max_angle);
+ limited_dir = side_rotation.xform(center_axis);
+ }
+
+ return limited_dir.normalized();
+}
+
+#ifdef TOOLS_ENABLED
+void JointLimitationCone3D::draw_shape(Ref &p_surface_tool, const Transform3D &p_transform, float p_bone_length, const Color &p_color) const {
+ static const int N = 16;
+ static const real_t DP = Math::TAU / (real_t)N;
+
+ real_t sphere_r = p_bone_length * (real_t)0.25;
+ if (sphere_r <= CMP_EPSILON) {
+ return;
+ }
+ real_t alpha = CLAMP((real_t)radius_range, (real_t)0.0, (real_t)1.0) * Math::PI;
+ real_t y_cap = sphere_r * Math::cos(alpha);
+ real_t r_cap = sphere_r * Math::sin(alpha);
+
+ LocalVector vts;
+
+ // Cone bottom.
+ if (r_cap > CMP_EPSILON) {
+ for (int i = 0; i < N; i++) {
+ real_t a0 = (real_t)i * DP;
+ real_t a1 = (real_t)((i + 1) % N) * DP;
+ Vector3 p0 = Vector3(r_cap * Math::cos(a0), y_cap, r_cap * Math::sin(a0));
+ Vector3 p1 = Vector3(r_cap * Math::cos(a1), y_cap, r_cap * Math::sin(a1));
+ vts.push_back(p0);
+ vts.push_back(p1);
+ }
+ }
+
+ // Rotate arcs around Y-axis.
+ real_t t_start;
+ real_t arc_len;
+ if (alpha <= (real_t)1e-6) {
+ t_start = (real_t)0.5 * Math::PI;
+ arc_len = Math::PI;
+ } else {
+ t_start = (real_t)0.5 * Math::PI + alpha;
+ arc_len = Math::PI - alpha;
+ }
+ real_t dt = arc_len / (real_t)N;
+
+ for (int k = 0; k < N; k++) {
+ Basis ry(Vector3(0, 1, 0), (real_t)k * DP);
+
+ Vector3 prev = ry.xform(Vector3(sphere_r * Math::cos(t_start), sphere_r * Math::sin(t_start), 0));
+
+ for (int s = 1; s <= N; s++) {
+ real_t t = t_start + dt * (real_t)s;
+ Vector3 cur = ry.xform(Vector3(sphere_r * Math::cos(t), sphere_r * Math::sin(t), 0));
+
+ vts.push_back(prev);
+ vts.push_back(cur);
+
+ prev = cur;
+ }
+
+ Vector3 mouth = ry.xform(Vector3(sphere_r * Math::cos(t_start), sphere_r * Math::sin(t_start), 0));
+ Vector3 center = Vector3();
+
+ vts.push_back(center);
+ vts.push_back(mouth);
+ }
+
+ // Stack rings.
+ for (int i = 1; i <= 3; i++) {
+ for (int sgn = -1; sgn <= 1; sgn += 2) {
+ real_t y = (real_t)sgn * sphere_r * ((real_t)i / (real_t)4.0);
+ if (y >= y_cap - CMP_EPSILON) {
+ continue;
+ }
+ real_t ring_r2 = sphere_r * sphere_r - y * y;
+ if (ring_r2 <= (real_t)0.0) {
+ continue;
+ }
+ real_t ring_r = Math::sqrt(ring_r2);
+
+ for (int j = 0; j < N; j++) {
+ real_t a0 = (real_t)j * DP;
+ real_t a1 = (real_t)((j + 1) % N) * DP;
+ Vector3 p0 = Vector3(ring_r * Math::cos(a0), y, ring_r * Math::sin(a0));
+ Vector3 p1 = Vector3(ring_r * Math::cos(a1), y, ring_r * Math::sin(a1));
+
+ vts.push_back(p0);
+ vts.push_back(p1);
+ }
+ }
+ }
+
+ for (int64_t i = 0; i < vts.size(); i++) {
+ p_surface_tool->set_color(p_color);
+ p_surface_tool->add_vertex(p_transform.xform(vts[i]));
+ }
+}
+#endif // TOOLS_ENABLED
diff --git a/scene/resources/3d/joint_limitation_cone_3d.h b/scene/resources/3d/joint_limitation_cone_3d.h
new file mode 100644
index 00000000000..a9200864b11
--- /dev/null
+++ b/scene/resources/3d/joint_limitation_cone_3d.h
@@ -0,0 +1,52 @@
+/**************************************************************************/
+/* joint_limitation_cone_3d.h */
+/**************************************************************************/
+/* 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. */
+/**************************************************************************/
+
+#pragma once
+
+#include "scene/resources/3d/joint_limitation_3d.h"
+
+class JointLimitationCone3D : public JointLimitation3D {
+ GDCLASS(JointLimitationCone3D, JointLimitation3D);
+
+ real_t radius_range = 0.25;
+
+protected:
+ static void _bind_methods();
+
+ virtual Vector3 _solve(const Vector3 &p_direction) const override;
+
+public:
+ void set_radius_range(real_t p_radius_range);
+ real_t get_radius_range() const;
+
+#ifdef TOOLS_ENABLED
+ virtual void draw_shape(Ref &p_surface_tool, const Transform3D &p_transform, float p_bone_length, const Color &p_color) const override;
+#endif // TOOLS_ENABLED
+};
diff --git a/scene/resources/curve.cpp b/scene/resources/curve.cpp
index 0e50c3d17d4..154859f63e6 100644
--- a/scene/resources/curve.cpp
+++ b/scene/resources/curve.cpp
@@ -2127,6 +2127,14 @@ PackedVector3Array Curve3D::get_baked_up_vectors() const {
return baked_up_vector_cache;
}
+Vector Curve3D::get_baked_dist_cache() const {
+ if (baked_cache_dirty) {
+ _bake();
+ }
+
+ return baked_dist_cache;
+}
+
Vector3 Curve3D::get_closest_point(const Vector3 &p_to_point) const {
// Brute force method.
diff --git a/scene/resources/curve.h b/scene/resources/curve.h
index 02e2e9168db..7782135402d 100644
--- a/scene/resources/curve.h
+++ b/scene/resources/curve.h
@@ -353,6 +353,7 @@ public:
PackedVector3Array get_baked_points() const; // Useful for going through.
Vector get_baked_tilts() const; //useful for going through
PackedVector3Array get_baked_up_vectors() const;
+ Vector get_baked_dist_cache() const;
Vector3 get_closest_point(const Vector3 &p_to_point) const;
real_t get_closest_offset(const Vector3 &p_to_point) const;
PackedVector3Array get_points() const;