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); }