• R/O
  • SSH
  • HTTPS

mmdx: Commit


Commit MetaInfo

Revision669 (tree)
Zeit2011-02-18 23:00:52
Autorwilfrem

Log Message

IKLimitter実装

Ändern Zusammenfassung

Diff

--- branches/XNA4/MikuMikuDanceXNA/Misc/DefaltIKLimitter.cs (revision 668)
+++ branches/XNA4/MikuMikuDanceXNA/Misc/DefaltIKLimitter.cs (revision 669)
@@ -2,18 +2,202 @@
22 using System.Collections.Generic;
33 using System.Linq;
44 using System.Text;
5+using Microsoft.Xna.Framework;
56
67 namespace MikuMikuDance.XNA.Misc
78 {
9+ /// <summary>
10+ /// 回転制限クラス
11+ /// </summary>
12+ internal class RotationLimit
13+ {
14+ /// <summary>
15+ /// 最大回転
16+ /// </summary>
17+ /// <remarks>X回転、Y回転、Z回転制限</remarks>
18+ public float[] MaxRot { get; protected set; }
19+ /// <summary>
20+ /// 最小回転
21+ /// </summary>
22+ /// <remarks>X回転、Y回転、Z回転制限</remarks>
23+ public float[] MinRot { get; protected set; }
24+ /// <summary>
25+ /// 角度の反射調整機能使用フラグ
26+ /// </summary>
27+ /// <remarks>IKのCCDソルブは足のIKが<が>になる感じの解を出してくるので、反射してやると上手くいくっぽい</remarks>
28+ public bool[] Mirror { get; private set; }
29+ /// <summary>
30+ /// 角度の反射調整の反発係数
31+ /// </summary>
32+ public float[] Restitution { get; private set; }
33+ /// <summary>
34+ /// 角速度の"粘性"係数。IKのソルブの過程で解が"飛ぶ"のを防ぐために設定
35+ /// </summary>
36+ public float[] Stickness { get; private set; }
37+
38+
39+ /// <summary>
40+ /// 既定のコンストラクタ
41+ /// </summary>
42+ public RotationLimit()
43+ {
44+ MaxRot = new float[3];
45+ MinRot = new float[3];
46+ Mirror = new bool[3];
47+ Restitution = new float[3];
48+ for (int i = 0; i < 3; i++)
49+ {
50+ MaxRot[i] = MathHelper.Pi;
51+ MinRot[i] = -MathHelper.Pi;
52+ Mirror[i] = false;
53+ Restitution[i] = 0.5f;
54+ }
55+
56+
57+ }
58+
59+
60+ /// <summary>
61+ /// 指定した角度をアジャストする
62+ /// </summary>
63+ /// <param name="value">回転角</param>
64+ /// <param name="index">回転軸</param>
65+ /// <returns>アジャスト済み角度</returns>
66+ public float Adjust(float value, int index)
67+ {
68+ if (MinRot[index] > MaxRot[index])
69+ {//角度が逆なら入れ替えておく
70+ float temp = MinRot[index];
71+ MinRot[index] = MaxRot[index];
72+ MaxRot[index] = temp;
73+ }
74+ if (MaxRot[index] < value)
75+ {
76+ if (Mirror[index])
77+ return MaxRot[index] * (1 + Restitution[index]) - value * Restitution[index];
78+ else
79+ return MaxRot[index];
80+ }
81+ else if (MinRot[index] > value)
82+ {
83+ if (Mirror[index])
84+ return MinRot[index] * (1 + Restitution[index]) - value * Restitution[index];
85+ else
86+ return MinRot[index];
87+ }
88+ else
89+ return value;
90+
91+ }
92+ }
893 public class DefaltIKLimitter : IIKLimitter
994 {
95+ /// <summary>
96+ /// 総合稼働軸制限一覧
97+ /// </summary>
98+ /// <remarks>ボーン名マッチング用の正規表現オブジェクトと許可回転軸(親ボーン基準)</remarks>
99+ Dictionary<string, RotationLimit> TotalRotationLimits { get; set; }
100+
101+ /// <summary>
102+ /// 既定のコンストラクタ
103+ /// </summary>
104+ public DefaltIKLimitter()
105+ {
106+ //総合稼働制限
107+ TotalRotationLimits = new Dictionary<string, RotationLimit>();
108+ RotationLimit limit;
109+ limit = new RotationLimit();
110+ limit.MaxRot[0] = MathHelper.Pi;
111+ limit.MinRot[0] = MathHelper.ToRadians(3f);//3度ぐらい制限を設けてやると上手くいく。
112+ limit.MinRot[1] = 0;
113+ limit.MaxRot[1] = 0;
114+ limit.MinRot[2] = 0;
115+ limit.MaxRot[2] = 0;
116+ limit.Mirror[0] = true;
117+ limit.Restitution[0] = 0.99f;
118+ TotalRotationLimits.Add("左ひざ", limit);
119+ limit = new RotationLimit();
120+ limit.MaxRot[0] = MathHelper.Pi;
121+ limit.MinRot[0] = MathHelper.ToRadians(3f);//3度ぐらい制限を設けてやると上手くいく。
122+ limit.MinRot[1] = 0;
123+ limit.MaxRot[1] = 0;
124+ limit.MinRot[2] = 0;
125+ limit.MaxRot[2] = 0;
126+ limit.Mirror[0] = true;
127+ limit.Restitution[0] = 0.99f;
128+ TotalRotationLimits.Add("右ひざ", limit);
129+
130+ //IKのソルブ及びそれの調整計算に関するメモ
131+ //上記数値調整計算及び各種数値設定はMMDの元コード推定(リバースエンジニアリング、逆コンパイラとかはしてないからRエンジニアって言うのか分からないけど)する過程で落ち着いている今のところの解です。
132+ //ほんとの解法は樋口さんが知ってるんだろうけどw
133+ //解法は今のところIK-CCD法がMMDにとって最適だと考えてます。
134+ //理由として
135+ //・ひざのボーンにIKソルブ時の角度制限が入っているっぽいので、ソルブにボーンの角度を扱う必要があること
136+ //・高速解法が必要であること(MMDが非常に軽いことと、イテレーションの存在とその回数を考えると、軽いアルゴリズムを使ってないとつじつまが合わない)
137+ //が上げられます
138+ //そこで、CCD,Particleかの二つで、角度を使い易かったCCDを選びました。
139+ //ひざの角度調整はCCDのクセを抑える理由もあって工夫してあります。
140+ //CCDのクセとして、正しい解が<だとしたら、>という解を出してくることが多いという問題があります。(><は足ですw)
141+ //そのために"反発係数"なる謎なパラメータを付けてますw
142+ //また、解がほとんどまっすぐな解を出す際に、|な感じの解で固定されてしまう問題があるため、3度ぐらい下限を入れています(どうも、MMDの方も入れてるっぽいけど、よく分からない……)
143+ //これは現在の推定結果です。もっと再現性が高い解があれば、改造して、ぜひ教えてください
144+ }
145+
146+ bool CheckNaN(Vector3 input)
147+ {
148+ if (float.IsNaN(input.X))
149+ return true;
150+ if (float.IsNaN(input.Y))
151+ return true;
152+ if (float.IsNaN(input.Z))
153+ return true;
154+ return false;
155+ }
156+
10157 #region IIKLimitter メンバー
11158
12159 public void Adjust(Model.MMDBone bone)
13160 {
14- throw new NotImplementedException();
15- }
161+ if (!TotalRotationLimits.ContainsKey(bone.Name))
162+ return;
163+ float YRot, XRot, ZRot;
164+ int FactoringType = 0;
165+ //if (MMDMath.FactoringQuaternionZXY(rot, out ZRot, out XRot, out YRot))
166+ //まずはXYZで分解
167+ if (!MMDMath.FactoringQuaternionXYZ(bone.LocalTransform.Rotation, out XRot, out YRot, out ZRot))
168+ {//ジンバルロック対策
169+ //YZXで分解
170+ if (!MMDMath.FactoringQuaternionYZX(bone.LocalTransform.Rotation, out YRot, out ZRot, out XRot))
171+ {
172+ //ZXYで分解
173+ MMDMath.FactoringQuaternionZXY(bone.LocalTransform.Rotation, out ZRot, out XRot, out YRot);
174+ FactoringType = 2;
175+ }
176+ else
177+ FactoringType = 1;
178+ }
179+ else
180+ FactoringType = 0;
16181
182+ RotationLimit lim = TotalRotationLimits[bone.Name];
183+ XRot = lim.Adjust(XRot, 0);
184+ YRot = lim.Adjust(YRot, 1);
185+ ZRot = lim.Adjust(ZRot, 2);
186+ if (FactoringType == 0)
187+ bone.LocalTransform.Rotation = Quaternion.CreateFromRotationMatrix(
188+ Matrix.CreateRotationX(XRot) *
189+ Matrix.CreateRotationY(YRot) *
190+ Matrix.CreateRotationZ(ZRot));
191+ else if (FactoringType == 1)
192+ bone.LocalTransform.Rotation = Quaternion.CreateFromRotationMatrix(
193+ Matrix.CreateRotationY(YRot) *
194+ Matrix.CreateRotationZ(ZRot) *
195+ Matrix.CreateRotationX(XRot));
196+ else
197+ bone.LocalTransform.Rotation = Quaternion.CreateFromYawPitchRoll(YRot, XRot, ZRot);
198+
199+ }
200+
17201 #endregion
18202 }
19203 }
--- branches/XNA4/MikuMikuDanceXNA/Misc/MMDMath.cs (nonexistent)
+++ branches/XNA4/MikuMikuDanceXNA/Misc/MMDMath.cs (revision 669)
@@ -0,0 +1,127 @@
1+using System;
2+using System.Collections.Generic;
3+using System.Linq;
4+using System.Text;
5+using Microsoft.Xna.Framework;
6+
7+namespace MikuMikuDance.XNA.Misc
8+{
9+ public static class MMDMath
10+ {
11+ /// <summary>
12+ /// クォータニオンをYaw(Y回転), Pitch(X回転), Roll(Z回転)に分解する関数
13+ /// </summary>
14+ /// <param name="input">分解するクォータニオン</param>
15+ /// <param name="ZRot">Z軸回転</param>
16+ /// <param name="XRot">X軸回転(-PI/2~PI/2)</param>
17+ /// <param name="YRot">Y軸回転</param>
18+ /// <returns>ジンバルロックが発生した時はfalse。ジンバルロックはX軸回転で発生</returns>
19+ public static bool FactoringQuaternionZXY(Quaternion input, out float ZRot, out float XRot, out float YRot)
20+ {
21+ //クォータニオンの正規化
22+ Quaternion inputQ = new Quaternion(input.X, input.Y, input.Z, input.W);
23+ inputQ.Normalize();
24+ //マトリクスを生成する
25+ Matrix rot = Matrix.CreateFromQuaternion(inputQ);
26+ //ヨー(X軸周りの回転)を取得
27+ if (rot.M32 > 1 - 1.0e-4 || rot.M32 < -1 + 1.0e-4)
28+ {//ジンバルロック判定
29+ XRot = (rot.M32 < 0 ? MathHelper.PiOver2 : -MathHelper.PiOver2);
30+ ZRot = 0; YRot = (float)Math.Atan2(-rot.M13, rot.M11);
31+ return false;
32+ }
33+ XRot = -(float)Math.Asin(rot.M32);
34+ //ロールを取得
35+ ZRot = (float)Math.Asin(rot.M12 / Math.Cos(XRot));
36+ if (float.IsNaN(ZRot))
37+ {//漏れ対策
38+ XRot = (rot.M32 < 0 ? MathHelper.PiOver2 : -MathHelper.PiOver2);
39+ ZRot = 0; YRot = (float)Math.Atan2(-rot.M13, rot.M11);
40+ return false;
41+ }
42+ if (rot.M22 < 0)
43+ ZRot = MathHelper.Pi - ZRot;
44+ //ピッチを取得
45+ YRot = (float)Math.Atan2(rot.M31, rot.M33);
46+ return true;
47+ }
48+ /// <summary>
49+ /// クォータニオンをX,Y,Z回転に分解する関数
50+ /// </summary>
51+ /// <param name="input">分解するクォータニオン</param>
52+ /// <param name="XRot">X軸回転</param>
53+ /// <param name="YRot">Y軸回転(-PI/2~PI/2)</param>
54+ /// <param name="ZRot">Z軸回転</param>
55+ /// <returns></returns>
56+ public static bool FactoringQuaternionXYZ(Quaternion input, out float XRot, out float YRot, out float ZRot)
57+ {
58+ //クォータニオンの正規化
59+ Quaternion inputQ = new Quaternion(input.X, input.Y, input.Z, input.W);
60+ inputQ.Normalize();
61+ //マトリクスを生成する
62+ Matrix rot = Matrix.CreateFromQuaternion(inputQ);
63+ //Y軸回りの回転を取得
64+ if (rot.M13 > 1 - 1.0e-4 || rot.M13 < -1 + 1.0e-4)
65+ {//ジンバルロック判定
66+ XRot = 0;
67+ YRot = (rot.M13 < 0 ? MathHelper.PiOver2 : -MathHelper.PiOver2);
68+ ZRot = -(float)Math.Atan2(-rot.M21, rot.M22);
69+ return false;
70+ }
71+ YRot = -(float)Math.Asin(rot.M13);
72+ //X軸回りの回転を取得
73+ XRot = (float)Math.Asin(rot.M23 / Math.Cos(YRot));
74+ if (float.IsNaN(XRot))
75+ {//ジンバルロック判定(漏れ対策)
76+ XRot = 0;
77+ YRot = (rot.M13 < 0 ? MathHelper.PiOver2 : -MathHelper.PiOver2);
78+ ZRot = -(float)Math.Atan2(-rot.M21, rot.M22);
79+ return false;
80+ }
81+ if (rot.M33 < 0)
82+ XRot = MathHelper.Pi - XRot;
83+ //Z軸回りの回転を取得
84+ ZRot = (float)Math.Atan2(rot.M12, rot.M11);
85+ return true;
86+ }
87+ /// <summary>
88+ /// クォータニオンをY,Z,X回転に分解する関数
89+ /// </summary>
90+ /// <param name="input">分解するクォータニオン</param>
91+ /// <param name="YRot">Y軸回転</param>
92+ /// <param name="ZRot">Z軸回転(-PI/2~PI/2)</param>
93+ /// <param name="XRot">X軸回転</param>
94+ /// <returns></returns>
95+ public static bool FactoringQuaternionYZX(Quaternion input, out float YRot, out float ZRot, out float XRot)
96+ {
97+ //クォータニオンの正規化
98+ Quaternion inputQ = new Quaternion(input.X, input.Y, input.Z, input.W);
99+ inputQ.Normalize();
100+ //マトリクスを生成する
101+ Matrix rot = Matrix.CreateFromQuaternion(inputQ);
102+ //Z軸回りの回転を取得
103+ if (rot.M21 > 1 - 1.0e-4 || rot.M21 < -1 + 1.0e-4)
104+ {//ジンバルロック判定
105+ YRot = 0;
106+ ZRot = (rot.M21 < 0 ? MathHelper.PiOver2 : -MathHelper.PiOver2);
107+ XRot = -(float)Math.Atan2(-rot.M32, rot.M33);
108+ return false;
109+ }
110+ ZRot = -(float)Math.Asin(rot.M21);
111+ //Y軸回りの回転を取得
112+ YRot = (float)Math.Asin(rot.M31 / Math.Cos(ZRot));
113+ if (float.IsNaN(YRot))
114+ {//ジンバルロック判定(漏れ対策)
115+ YRot = 0;
116+ ZRot = (rot.M21 < 0 ? MathHelper.PiOver2 : -MathHelper.PiOver2);
117+ XRot = -(float)Math.Atan2(-rot.M32, rot.M33);
118+ return false;
119+ }
120+ if (rot.M11 < 0)
121+ YRot = MathHelper.Pi - YRot;
122+ //X軸回りの回転を取得
123+ XRot = (float)Math.Atan2(rot.M23, rot.M22);
124+ return true;
125+ }
126+ }
127+}
Show on old repository browser