Esempio n. 1
0
        /// <summary>
        /// IKTo get the link based effector 1 position
        /// </summary>
        /// <param name="ikLink">IKLink</param>
        /// <param name="effector">Effector</param>
        /// <returns>IKLink based effector 1 position</returns>
        private Vector3 GetLink2Effector(IkLink ikLink, PMXBone effector)
        {
            var ToLinkLocal = Matrix.Invert(ikLink.ikLinkBone.GlobalPose);
            var effectorPos = Vector3.TransformCoordinate(effector.Position, effector.GlobalPose * ToLinkLocal); //●

            return(Vector3.Normalize(effectorPos - ikLink.ikLinkBone.Position));
        }
Esempio n. 2
0
        /// <summary>
        /// To limit the amount of rotation
        /// </summary>
        /// <param name="ikLink">IKLink</param>
        private void RestrictRotation(IkLink ikLink)
        {
            if (!ikLink.isLimited)
            {
                return;
            }

            float xRotation, yRotation, zRotation;
            var   type    = SplitRotation(ikLink.ikLinkBone.Rotation, out xRotation, out yRotation, out zRotation);
            var   clamped = Vector3.Clamp(new Vector3(xRotation, yRotation, zRotation).NormalizeEular(), ikLink.minRot, ikLink.maxRot);

            xRotation = clamped.X;
            yRotation = clamped.Y;
            zRotation = clamped.Z;
            switch (type)
            {
            case 0:
                ikLink.ikLinkBone.Rotation = Quaternion.RotationMatrix(Matrix.RotationX(xRotation) * Matrix.RotationY(yRotation) * Matrix.RotationZ(zRotation));
                break;

            case 1:
                ikLink.ikLinkBone.Rotation = Quaternion.RotationMatrix(Matrix.RotationY(yRotation) * Matrix.RotationZ(zRotation) * Matrix.RotationX(xRotation));
                break;

            case 2:
                ikLink.ikLinkBone.Rotation = Quaternion.RotationYawPitchRoll(yRotation, xRotation, zRotation);
                break;
            }
        }
Esempio n. 3
0
        /// <summary>
        /// IKTo get the link relative target position
        /// </summary>
        /// <param name="ikLink">IKLink</param>
        /// <param name="TargetGlobalPos">Global relative target position</param>
        /// <returns>IKTarget position of a standard link</returns>
        private Vector3 GetLink2Target(IkLink ikLink, Vector3 TargetGlobalPos)
        {
            var     ToLinkLocal = Matrix.Invert(ikLink.ikLinkBone.GlobalPose);
            Vector3 targetPos;

            Vector3.TransformCoordinate(ref TargetGlobalPos, ref ToLinkLocal, out targetPos);
            return(Vector3.Normalize(targetPos - ikLink.ikLinkBone.Position));
        }
Esempio n. 4
0
        /*
         * CCD-IKのIKLink計算の仕組み
         *-◎-◎-◎-●
         * ↑関節      ▲←IKターゲット(MMD内ではIKBoneと呼ぶ)。関節を近づけたい目標
         * (関節内の◎にあたる、IKで曲げる途中の関節をIKリンク、●にあたる関節の終点をエフェクタ(MMD内ではターゲットボーン)
         * とする。
         * 1)まず、◎-●の方向ベクタを求める。ベクトルA
         * 2)次に、◎-▲の方向ベクタを求める。ベクトルB
         * 3)∠●◎▲における最短回転角度を求める。arccos(A・B)よりラジアン単位で求まる
         * 4)2つの方向ベクトルの外積から回転軸を求める
         */
        /// <summary>
        /// IKLinkCalculation
        /// </summary>
        /// <param name="ikLink">IKLink</param>
        /// <param name="link2Effector">IKLink based effector position</param>
        /// <param name="link2Target">IKTarget position of a standard link</param>
        /// <param name="RotationLimited">The absolute value of the upper and lower limits for the angle of rotation</param>
        private void IKLinkCalc(IkLink ikLink, Vector3 link2Effector, Vector3 link2Target, float RotationLimited)
        {
            //To determine the rotation angle
            var dot = Vector3.Dot(link2Effector, link2Target);

            if (dot > 1f)
            {
                dot = 1f;
            }
            var rotationAngle = ClampFloat((float)Math.Acos(dot), RotationLimited);

            if (float.IsNaN(rotationAngle))
            {
                return;
            }
            if (rotationAngle <= 1.0e-3f)
            {
                return;
            }

            //Ask the axis
            var rotationAxis = Vector3.Cross(link2Effector, link2Target);

            ikLink.loopCount++;

            //Builds a matrix that rotates around axis。
            var rotation = Quaternion.RotationAxis(rotationAxis, rotationAngle);

            rotation.Normalize();
            ikLink.ikLinkBone.Rotation = rotation * ikLink.ikLinkBone.Rotation;

            #region Rotation amount limit
            RestrictRotation(ikLink);
            #endregion

            ikLink.ikLinkBone.UpdateGrobalPose();
        }
Esempio n. 5
0
        /*
         * CCD-IKのIKLink計算の仕組み
         *-◎-◎-◎-●
         * ↑関節      ▲←IKターゲット(MMD内ではIKBoneと呼ぶ)。関節を近づけたい目標
         * (関節内の◎にあたる、IKで曲げる途中の関節をIKリンク、●にあたる関節の終点をエフェクタ(MMD内ではターゲットボーン)
         * とする。
         * 1)まず、◎-●の方向ベクタを求める。ベクトルA
         * 2)次に、◎-▲の方向ベクタを求める。ベクトルB
         * 3)∠●◎▲における最短回転角度を求める。arccos(A・B)よりラジアン単位で求まる
         * 4)2つの方向ベクトルの外積から回転軸を求める
         */
        /// <summary>
        /// IKLinkを計算
        /// </summary>
        /// <param name="ikLink">IKリンク</param>
        /// <param name="link2Effector">IKリンク基準のエフェクタ位置</param>
        /// <param name="link2Target">IKリンク基準のターゲット位置</param>
        /// <param name="RotationLimited">回転角度の上下限値の絶対値</param>
        private void IKLinkCalc(IkLink ikLink, Vector3 link2Effector, Vector3 link2Target, float RotationLimited)
        {
            //回転角度を求める
            var dot = Vector3.Dot(link2Effector, link2Target);

            if (dot > 1f)
            {
                dot = 1f;
            }
            var rotationAngle = ClampFloat((float)Math.Acos(dot), RotationLimited);

            if (float.IsNaN(rotationAngle))
            {
                return;
            }
            if (rotationAngle <= 1.0e-3f)
            {
                return;
            }

            //回転軸を求める
            var rotationAxis = Vector3.Cross(link2Effector, link2Target);

            ikLink.loopCount++;

            //軸を中心として回転する行列を作成する。
            var rotation = Quaternion.RotationAxis(rotationAxis, rotationAngle);

            rotation.Normalize();
            ikLink.ikLinkBone.Rotation = rotation * ikLink.ikLinkBone.Rotation;

            #region 回転量制限
            RestrictRotation(ikLink);
            #endregion

            ikLink.ikLinkBone.UpdateGrobalPose();
        }