private void LateUpdate()
{
if (applyTransforms)
{
if (target == null)
{
return;
}
// Get the chain and assert that it is not broken
Chain c = ChainManager.BuildChain(root);
Debug.Assert(!c.IsBroken());
List <Joint> joints = new List <Joint>();
for (int i = 0; i < c.BoneCount; i++)
{
joints.Add(c.GetBone(i).Base);
}
joints.Add(c.EndEffector);
Chain_FABRIK.Solve(c, target);
ChainManager.ApplyJointTransforms(root, joints);
visualizer.Show();
visualizer.SetChain(c);
}
}
void LateUpdate()
{
if (runIK)
{
runIK = false;
if (Target == null)
{
return;
}
// Get the chain and assert that it is not broken
Chain chain = ChainManager.BuildChain(ChainRoot);
Debug.Assert(!chain.IsBroken());
var chainCopy = chain.Copy();
chainCopy.Destroy();
chain.Restore();
Chain_FABRIK.Solve(chain, Target);
ChainManager.ApplyJointTransforms(ChainRoot, chain.GetJoints());
chain = ChainManager.BuildChain(ChainRoot);
ConstraintsUtil.Constrain(chain, Target);
ChainManager.ApplyJointTransforms(ChainRoot, chain.GetJoints());
}
if (Input.GetKeyDown(KeyCode.I))
{
runIK = false;
Chain chain = ChainManager.BuildChain(ChainRoot);
chain.Restore();
Chain_FABRIK.Solve(chain, Target);
ChainManager.ApplyJointTransforms(ChainRoot, chain.GetJoints());
}
if (Input.GetKeyDown(KeyCode.R))
{
runIK = false;
Chain chain = ChainManager.BuildChain(ChainRoot);
chain.Restore();
ChainManager.ApplyJointTransforms(ChainRoot, chain.GetJoints());
}
if (Input.GetKeyDown(KeyCode.C))
{
Chain chain = ChainManager.BuildChain(ChainRoot);
ConstraintsUtil.Constrain(chain, Target);
ChainManager.ApplyJointTransforms(ChainRoot, chain.GetJoints());
}
if (Input.GetKeyDown(KeyCode.D))
{
Chain chain = ChainManager.BuildChain(ChainRoot);
DisplayChainInfo(chain);
}
}
// Pre: The end effector NEVER defines a constraint!!!
// Pre: c is a "solved" chain, where the endEffector is at the Target pos
public static void Constrain(Chain chain, Transform target, int startingJoint = 0)
{
DetectConstraint:
// Detect the first constraint not satisfied, starting from startJoint
var detection = Detect(chain, startingJoint);
// If we don't detect anything, we are done
if (detection == null)
{
return;
}
// Verify that the detection is on a correct Joint
Debug.Assert(detection.index != chain.BoneCount);
// Apply the constraint using the detection info
Apply(chain, detection);
// At this point detection.jointIndex satisfies the constraint
if (detection.index + 1 >= chain.BoneCount)
{
//Debug.Log("DEADLOCK");
detection.index = 0;
IList <Joint> joints = chain.GetJoints();
Quaternion relative = Quat.Relative(joints[0].Orientation, joints[0].RestOrientation);
chain.GetBone(0).Rotate(relative);
chain.GetBone(0).Rotate(relative);
//float randAngle = Random.Range(-joints[0].MaxAngleCW, joints[0].MaxAngleCCW);
//float randAngle = -joints[0].MaxAngleCW;
//chain.GetBone(0).Rotate(relative);
//chain.GetBone(0).Rotate(Quaternion.AngleAxis(randAngle, Quat.GetAxis(relative)));
for (int i = 1; i < chain.BoneCount; i++)
{
chain.GetBone(i).MoveTo(chain.GetBone(i - 1).Tip.Position);
}
goto DetectConstraint;
}
// Apply FABRIK starting from the next joint after the constrained joint
Chain_FABRIK.Solve(chain, target, detection.index + 1);
startingJoint = detection.index + 1;
goto DetectConstraint;
}
private void Update()
{
if (Input.GetKeyDown(KeyCode.Space))
{
show = !show;
}
if (Input.GetKeyDown(KeyCode.I))
{
// Apply IK
Chain c = visualizer.GetChain();
c.GetBone(1).Move(new Vector3(1, 0, 0));
visualizer.SetChain(c);
}
if (Input.GetKeyDown(KeyCode.Alpha1))
{
Chain c = visualizer.GetChain();
c.AttachBoneToParent(1);
visualizer.SetChain(c);
}
if (Input.GetKeyDown(KeyCode.Alpha2))
{
Chain c = visualizer.GetChain();
c.AttachBoneToParent(1, false);
visualizer.SetChain(c);
}
if (Input.GetKeyDown(KeyCode.Alpha3))
{
Chain c = visualizer.GetChain();
c.AttachBoneToChild(0);
visualizer.SetChain(c);
}
if (Input.GetKeyDown(KeyCode.Alpha4))
{
Chain c = visualizer.GetChain();
c.AttachBoneToChild(0, false);
visualizer.SetChain(c);
}
if (Input.GetKeyDown(KeyCode.F))
{
if (target == null)
{
return;
}
Chain c = visualizer.GetChain();
Chain_FABRIK.Solve(c, target);
visualizer.SetChain(c);
}
if (Input.GetKeyDown(KeyCode.A))
{
applyTransforms = !applyTransforms;
}
if (show)
{
visualizer.Show();
}
else
{
visualizer.Hide();
}
}
