Tanoda
FastIKDobot.cs
Go to the documentation of this file.
1#if UNITY_EDITOR
2using UnityEditor;
3#endif
4using UnityEngine;
5
7{
11 public class FastIKDobot : MonoBehaviour
12 {
16 public int ChainLength = 2;
17
21 public Transform Target;
22 public Transform Pole;
23
24 public Vector3 Offset;
25 public byte Mode;
26
27 public bool keepLocalX = true;
28
32 [Header("Solver Parameters")]
33 public int Iterations = 10;
34
38 public float Delta = 0.001f;
39
43 [Range(0, 1)]
44 public float SnapBackStrength = 1f;
45
46 public bool UseLerp = true;
47
48 public float LerpStep = 0.1f;
49
50 public bool ResolveNow = true;
51
52
53 protected float[] BonesLength; //Target to Origin
54 protected float CompleteLength;
55 protected Transform[] Bones;
56 protected Vector3[] Positions;
57 protected Vector3[] StartDirectionSucc;
58 protected Quaternion[] StartRotationBone;
59 protected Quaternion StartRotationTarget;
60 protected Transform Root;
61
62
63 // Start is called before the first frame update
64 void Awake()
65 {
66 Init();
67 }
68
69 void Init()
70 {
71
72 //initial array
73 Bones = new Transform[ChainLength + 1];
74 Positions = new Vector3[ChainLength + 1];
75 BonesLength = new float[ChainLength];
76 StartDirectionSucc = new Vector3[ChainLength + 1];
77 StartRotationBone = new Quaternion[ChainLength + 1];
78
79 //find root
80 Root = transform;
81 for (var i = 0; i <= ChainLength; i++)
82 {
83 if (Root == null)
84 throw new UnityException("The chain value is longer than the ancestor chain!");
85 Root = Root.parent;
86 }
87
88 //init target
89 if (Target == null)
90 {
91 Target = new GameObject(gameObject.name + " Target").transform;
92 SetPositionRootSpace(Target, GetPositionRootSpace(transform));
93 }
94 StartRotationTarget = GetRotationRootSpace(Target);
95
96 //init data
97 var current = transform;
99
100 for (var i = Bones.Length - 1; i >= 0; i--)
101 {
102 Bones[i] = current;
103 StartRotationBone[i] = GetRotationRootSpace(current);
104
105 if (i == Bones.Length - 1)
106 {
107 //leaf
108 StartDirectionSucc[i] = GetPositionRootSpace(Target) - GetPositionRootSpace(current);
109 }
110 else
111 {
112 //mid bone
113 StartDirectionSucc[i] = GetPositionRootSpace(Bones[i + 1]) - GetPositionRootSpace(current);
114 BonesLength[i] = StartDirectionSucc[i].magnitude;
116 }
117
118 current = current.parent;
119 }
120 }
121
122 // Update is called once per frame
123 void LateUpdate()
124 {
125 ResolveIK();
126 }
127
128 private void ResolveIK()
129 {
130 if (Target == null)
131 return;
132
133 if (BonesLength.Length != ChainLength)
134 Init();
135
136 //Fabric
137
138 // root
139 // (bone0) (bonelen 0) (bone1) (bonelen 1) (bone2)...
140 // x--------------------x--------------------x---...
141
142 //get position
143 for (int i = 0; i < Bones.Length; i++)
144 Positions[i] = GetPositionRootSpace(Bones[i]);
145
146 var targetPosition = GetPositionRootSpace(Target);
147 var targetRotation = GetRotationRootSpace(Target);
148
149 //1st is possible to reach?
150 if ((targetPosition - GetPositionRootSpace(Bones[0])).sqrMagnitude >= CompleteLength * CompleteLength)
151 {
152 //just strech it
153 var direction = (targetPosition - Positions[0]).normalized;
154 //set everything after root
155 for (int i = 1; i < Positions.Length; i++)
156 Positions[i] = Positions[i - 1] + direction * BonesLength[i - 1];
157 }
158 else
159 {
160 for (int i = 0; i < Positions.Length - 1; i++)
162
163 for (int iteration = 0; iteration < Iterations; iteration++)
164 {
165 //https://www.youtube.com/watch?v=UNoX65PRehA
166 //back
167 for (int i = Positions.Length - 1; i > 0; i--)
168 {
169 if (i == Positions.Length - 1)
170 Positions[i] = targetPosition; //set it to target
171 else
172 Positions[i] = Positions[i + 1] + (Positions[i] - Positions[i + 1]).normalized * BonesLength[i]; //set in line on distance
173 }
174
175 //forward
176 for (int i = 1; i < Positions.Length; i++)
177 Positions[i] = Positions[i - 1] + (Positions[i] - Positions[i - 1]).normalized * BonesLength[i - 1];
178
179 //close enough?
180 if ((Positions[Positions.Length - 1] - targetPosition).sqrMagnitude < Delta * Delta)
181 break;
182 }
183 }
184
185 //move towards pole
186 if (Pole != null)
187 {
188 var polePosition = GetPositionRootSpace(Pole);
189 for (int i = 1; i < Positions.Length - 1; i++)
190 {
191 var plane = new Plane(Positions[i + 1] - Positions[i - 1], Positions[i - 1]);
192 var projectedPole = plane.ClosestPointOnPlane(polePosition);
193 var projectedBone = plane.ClosestPointOnPlane(Positions[i]);
194 var angle = Vector3.SignedAngle(projectedBone - Positions[i - 1], projectedPole - Positions[i - 1], plane.normal);
195 Positions[i] = Quaternion.AngleAxis(angle, plane.normal) * (Positions[i] - Positions[i - 1]) + Positions[i - 1];
196 }
197 }
198
199 //set position & rotation
200 if (ResolveNow)
201 for (int i = 0; i < Positions.Length; i++)
202 {
203 if (i == Positions.Length - 1)
204 SetRotationRootSpace(Bones[i], Quaternion.Inverse(targetRotation) * StartRotationTarget * Quaternion.Inverse(StartRotationBone[i]));
205 else
206 {
207 SetRotationRootSpace(Bones[i],
208 Quaternion.FromToRotation(StartDirectionSucc[i], Positions[i + 1] - Positions[i]) *
209 Quaternion.Inverse(StartRotationBone[i]));
210 }
211 SetPositionRootSpace(Bones[i], Positions[i]);
212 }
213
214 if (ResolveNow)
215 {
216 Bones[Positions.Length - 1].localEulerAngles += Offset;
217 var tmp = Bones[Positions.Length - 1].localEulerAngles;
218 switch (Mode)
219 {
220 case 0:
221 Bones[Positions.Length - 1].localEulerAngles = new Vector3(-tmp.x ,tmp.y,tmp.z);
222 break;
223 case 1:
224 Bones[Positions.Length - 1].localEulerAngles = new Vector3(tmp.y ,tmp.x,tmp.z);
225 break;
226 case 2:
227 Bones[Positions.Length - 1].localEulerAngles = new Vector3(tmp.z ,tmp.y,tmp.x);
228 break;
229 case 3:
230 Bones[Positions.Length - 1].localEulerAngles = new Vector3(tmp.y ,tmp.z,tmp.x);
231 break;
232 case 4:
233 Bones[Positions.Length - 1].localEulerAngles = new Vector3(tmp.x ,tmp.z,tmp.y);
234 break;
235 }
236
237 if (keepLocalX)
238 {
239 transform.forward = Vector3.down;
240 }
241 }
242 }
243
244 private Vector3 GetPositionRootSpace(Transform current)
245 {
246 if (Root == null)
247 return current.position;
248 else
249 return Quaternion.Inverse(Root.rotation) * (current.position - Root.position);
250 }
251
252 private void SetPositionRootSpace(Transform current, Vector3 position)
253 {
254 if (Root == null)
255 current.position = UseLerp ? Vector3.Lerp(current.position, position, LerpStep) : position;
256 else
257 current.position = UseLerp ? Vector3.Lerp(current.position, Root.rotation * position + Root.position, LerpStep) : Root.rotation * position + Root.position;
258 }
259
260 private Quaternion GetRotationRootSpace(Transform current)
261 {
262 //inverse(after) * before => rot: before -> after
263 if (Root == null)
264 return current.rotation;
265 else
266 return Quaternion.Inverse(current.rotation) * Root.rotation;
267 }
268
269 private void SetRotationRootSpace(Transform current, Quaternion rotation)
270 {
271 if (Root == null)
272 current.rotation = rotation;
273 else
274 current.rotation = Root.rotation * rotation;
275 }
276
277 void OnDrawGizmos()
278 {
279#if UNITY_EDITOR
280 var current = this.transform;
281 for (int i = 0; i < ChainLength && current != null && current.parent != null; i++)
282 {
283 var scale = Vector3.Distance(current.position, current.parent.position) * 0.1f;
284 Handles.matrix = Matrix4x4.TRS(current.position, Quaternion.FromToRotation(Vector3.up, current.parent.position - current.position), new Vector3(scale, Vector3.Distance(current.parent.position, current.position), scale));
285 Handles.color = Color.green;
286 Handles.DrawWireCube(Vector3.up * 0.5f, Vector3.one);
287 current = current.parent;
288 }
289#endif
290 }
291
292 }
293}
UnityEngine.Color Color
Definition: TestScript.cs:32
int Iterations
Solver iterations per update
Definition: FastIKDobot.cs:33
int ChainLength
Chain length of bones
Definition: FastIKDobot.cs:16
float SnapBackStrength
Strength of going back to the start position.
Definition: FastIKDobot.cs:44
float Delta
Distance when the solver stops
Definition: FastIKDobot.cs:38
Transform Target
Target the chain should bent to
Definition: FastIKDobot.cs:21