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