public Bone(Vector start, Vector end, Bone parent)
{
StartPosition = start;
// set lenght as the euclidean distance between the starting and endpoint
Length = Vector.Distance(start, end);
Angle = Angles.ComputeAngle(start, end);
// set parent references
Parent = parent;
if (parent != null)
parent._children.Add(this);
}
/// <summary>
/// Performs a step of inverse kinematics using jacobian relaxation
/// </summary>
private void InverseKinematicsStep(List<Bone> ikSequence, Bone endEffector, Vector target)
{
var jacobian = new double[2, ikSequence.Count];
for (var i = 0; i < ikSequence.Count; ++i)
{
var v = ikSequence[i].EndPosition - ikSequence[i].StartPosition;
v /= v.Length;
v.Z = 1;
// compute partial derivatives
var partderiv = v % (target - ikSequence[i].StartPosition);
jacobian[0, i] = partderiv.X;
jacobian[1, i] = partderiv.Y;
}
var e = target - endEffector.EndPosition;
double[,] relaxed = null, transposed;
switch (Algorithm)
{
case IKAlgorithm.JacobianPseudoInverse:
relaxed = AM.PseudoInverse(jacobian);
break;
case IKAlgorithm.JacobianTranspose:
transposed = AM.Transpose(jacobian);
relaxed = AM.Multiply(0.00001, transposed);
break;
case IKAlgorithm.LeastSquares:
transposed = AM.Transpose(jacobian);
var inv = AM.PseudoInverse(AM.Add(AM.Multiply(jacobian, transposed), AM.Multiply(0.1, AM.Identity(2))));
relaxed = AM.Multiply(transposed, inv);
break;
}
var angles = Accord.Math.Matrix.Multiply(relaxed, new[] {e.X, e.Y});
for (var i = 0; i < ikSequence.Count; ++i)
{
ikSequence[i]._angle = Angles.AdjustAngle(ikSequence[i]._angle + angles[i]);
ikSequence[i]._children.ForEach(x => x._startPosition = ikSequence[i].EndPosition);
}
}
/// <summary>
/// Returns a list of all bones between the the current and
/// bone 'desc', which is a descendant of the current
/// </summary>
private List<Bone> GatherAncestorsBetween(Bone desc)
{
var bones = new List<Bone>();
while (desc != null)
{
bones.Add(desc);
if (desc == this) return bones;
desc = desc.Parent;
}
return null;
}
/// <summary>
/// Performs inverse kinematics, using the current bone as the starting point of the IK sequence
/// </summary>
public void InverseKinematics(Bone endEffector, Vector target)
{
// if previous task is in progress, stop it
if (_animThread != null && _animThread.IsBusy)
_animThread.CancelAsync();
var bones = GatherAncestorsBetween(endEffector);
if (bones == null) return;
var angles = bones.Select(x => x.Angle).ToArray();
// start optimization
var iterations = 0;
while (Vector.Distance(target, endEffector.EndPosition)
> DistanceLimit && iterations++ < IterationLimit)
{
InverseKinematicsStep(bones, endEffector, target);
}
// compute new angles
var anglesNew = bones.Select(x => x.Angle).ToArray();
var angleDeltas = new double[bones.Count];
for (var i = 0; i < bones.Count; ++i)
{
bones[i]._angle = angles[i];
// use the smaller angle to animate
double angle1 = Angles.AdjustAngle(anglesNew[i] - angles[i]),
angle2 = Angles.AdjustAngle(angles[i] - anglesNew[i]);
angleDeltas[i] = angle1 < angle2
? angle1 / AnimationSteps : -angle2 / AnimationSteps;
}
var worker = new BackgroundWorker { WorkerSupportsCancellation = true };
// apply new angles gradually - on a new thread, so the main can handle drawing
worker.DoWork += (s, e) =>
{
var thread = (BackgroundWorker)s;
for (var j = 0; j < AnimationSteps; ++j)
{
for (var i = 0; i < bones.Count; ++i)
{
bones[i].Angle += angleDeltas[i];
bones[i]._children.ForEach(x => x.StartPosition = bones[i].EndPosition);
}
ForwardKinematics(Angle);
Thread.Sleep(20);
if (thread.CancellationPending) break;
}
};
worker.RunWorkerAsync();
_animThread = worker;
}
/// <summary>
/// Select given bone for inverse kinematics. If its a third, unselect oldest
/// </summary>
public static void SelectNewInverse(Bone b)
{
b.IsInverseSelected = !b.IsInverseSelected;
if (_selectedBones.Contains(b))
_selectedBones.Remove(b);
else
{
if (_selectedBones.Count > 1)
{
_selectedBones[0].IsInverseSelected = false;
_selectedBones.Remove(_selectedBones.First());
}
_selectedBones.Add(b);
if (_selectedBones.Count != 2) return;
// check conflicts
_selectedBones[0].IsInverseConflicted = _selectedBones[1].IsInverseConflicted
= _conflicted = false;
if (_selectedBones.Count < 2) return;
if (_selectedBones[0].GatherAncestorsBetween(_selectedBones[1]) != null) return; // ok
if (_selectedBones[1].GatherAncestorsBetween(_selectedBones[0]) != null)
_selectedBones = new List<Bone> {_selectedBones[1], _selectedBones[0]};
else
_selectedBones[0].IsInverseConflicted = _selectedBones[1].IsInverseConflicted
= _conflicted = true;
}
}
private void OnEndPointClickCommand(object value)
{
var bone = (Bone)value;
_selectedEndPointBone = bone;
ClickPoint = MovePoint = bone.EndPosition;
}
private void OnClearCommad(object value)
{
Bones.Clear();
Bone.ClearState();
ClickPoint = TargetPoint = MovePoint = null;
_mainBone = _selectedEndPointBone = _selectedBone = null;
RaisePropertyChanged(() => MainPoint);
}