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);
}
}
public static bool StartInverseKinematics(Vector target)
{
if (_selectedBones.Count != 2 || _conflicted) return false;
_selectedBones[0].InverseKinematics(
_selectedBones[1], target);
return true;
}
/// <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>
/// Computes the angle between two points
/// </summary>
public static double ComputeAngle(Vector v1, Vector v2)
{
return AdjustAngle(Math.Atan2(v2.Y - v1.Y, v2.X - v1.X));
}
private void OnClearCommad(object value)
{
Bones.Clear();
Bone.ClearState();
ClickPoint = TargetPoint = MovePoint = null;
_mainBone = _selectedEndPointBone = _selectedBone = null;
RaisePropertyChanged(() => MainPoint);
}
public void HandleMouseMove(MouseEventArgs e, Point position)
{
// righ drag rearranges the structure by modifiing the selected bone's rotation angle
if (e.RightButton == MouseButtonState.Pressed)
{
if (_selectedBone != null)
_selectedBone.ForwardKinematics(Angles.ComputeAngle(
_selectedBone.StartPosition, new Vector(position.X, position.Y)));
TargetPoint = null;
}
else
{
if (_selectedBone != null)
{
_selectedBone.IsSelected = false;
_selectedBone = null;
}
if (ClickPoint != null)
MovePoint = new Vector(position.X, position.Y);
}
}
public void HandleMouseClick(MouseButtonEventArgs e, Point position)
{
if (e.ChangedButton != MouseButton.Left) return;
// first click is a special case since the main structure hasn't been created yet
if (ClickPoint == null && Bones.Count == 0)
{
MovePoint = ClickPoint = new Vector(position.X, position.Y);
return;
}
if (ClickPoint != null)
{
// a bone has been created
Bones.Add(new Bone(ClickPoint, MovePoint, _selectedEndPointBone));
if (_mainBone == null)
{
_mainBone = Bones.First();
RaisePropertyChanged(() => MainPoint);
}
MovePoint = ClickPoint = null;
}
else
{
// set target for inverse kinematics and start algorithm if bones are selected
var target = new Vector(position.X, position.Y);
if (Bone.StartInverseKinematics(target))
TargetPoint = target;
}
}
/// <summary>
/// Computes euclidean distance between two points
/// </summary>
public static double Distance(Vector a, Vector b)
{
return Math.Sqrt((a.X - b.X)*(a.X - b.X) +
(a.Y - b.Y) * (a.Y - b.Y) + (a.Z - b.Z) * (a.Z - b.Z));
}