コード例 #1
0
ファイル: Bone.cs プロジェクト: dobsapetike/InverseKinematics
        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);
        }
コード例 #2
0
ファイル: Bone.cs プロジェクト: dobsapetike/InverseKinematics
        /// <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);
            }
        }
コード例 #3
0
ファイル: Bone.cs プロジェクト: dobsapetike/InverseKinematics
 public static bool StartInverseKinematics(Vector target)
 {
     if (_selectedBones.Count != 2 || _conflicted) return false;
     _selectedBones[0].InverseKinematics(
         _selectedBones[1], target);
     return true;
 }
コード例 #4
0
ファイル: Bone.cs プロジェクト: dobsapetike/InverseKinematics
        /// <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;
        }
コード例 #5
0
 /// <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));
 }
コード例 #6
0
        private void OnClearCommad(object value)
        {
            Bones.Clear();
            Bone.ClearState();

            ClickPoint = TargetPoint = MovePoint = null;
            _mainBone = _selectedEndPointBone = _selectedBone = null;
            RaisePropertyChanged(() => MainPoint);
        }
コード例 #7
0
 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);
     }
 }
コード例 #8
0
        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;
            }
        }
コード例 #9
0
 /// <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));
 }