/// <summary>
/// 補間を行います。
/// </summary>
/// <param name="mat0">行列0</param>
/// <param name="mat1">行列1</param>
/// <param name="mat2">行列2</param>
/// <param name="mat3">行列3</param>
/// <param name="length">分割数</param>
/// <param name="p1">補間速度係数</param>
/// <returns>分割数だけTMOMatを持つ配列</returns>
public static TMOMat[] Slerp(TMOMat mat0, TMOMat mat1, TMOMat mat2, TMOMat mat3, int length, float p1)
{
TMOMat[] ret = new TMOMat[length];
Matrix m1 = mat1.m;
Matrix m2 = mat2.m;
Vector3 scaling1;
Vector3 scaling2;
Vector3 v1 = Helper.DecomposeMatrix(ref m1, out scaling1);
Vector3 v2 = Helper.DecomposeMatrix(ref m2, out scaling2);
Quaternion q1 = Quaternion.RotationMatrix(m1);
Quaternion q2 = Quaternion.RotationMatrix(m2);
Vector3 v0 = new Vector3(mat0.m.M41, mat0.m.M42, mat0.m.M43);
//Vector3 v1 = new Vector3(mat1.m.M41, mat1.m.M42, mat1.m.M43);
//Vector3 v2 = new Vector3(mat2.m.M41, mat2.m.M42, mat2.m.M43);
Vector3 v3 = new Vector3(mat3.m.M41, mat3.m.M42, mat3.m.M43);
float p0 = 0.0f;
float p2 = 1.0f;
float dt = 1.0f / length;
for (int i = 0; i < length; i++)
{
float t = dt * i;
float p = t * t * (p2 - 2 * p1 + p0) + t * (2 * p1 - 2 * p0) + p0;
Matrix m = Matrix.Scaling(Vector3.Lerp(scaling1, scaling2, p)) * Matrix.RotationQuaternion(Quaternion.Slerp(q1, q2, p)) * Matrix.Translation(Vector3.CatmullRom(v0, v1, v2, v3, p));
ret[i] = new TMOMat(ref m);
}
return(ret);
}
/// <summary>
/// フレームを補間します。
/// </summary>
/// <param name="frame0">フレーム0</param>
/// <param name="frame1">フレーム1</param>
/// <param name="frame2">フレーム2</param>
/// <param name="frame3">フレーム3</param>
/// <param name="length">分割数</param>
/// <param name="p1">補間速度係数</param>
/// <param name="id_pair">node idのペア</param>
/// <returns></returns>
public static TMOFrame[] Slerp(TMOFrame frame0, TMOFrame frame1, TMOFrame frame2, TMOFrame frame3, int length, float p1, int[] id_pair)
{
TMOFrame[] frames = new TMOFrame[length];
for (int frame_index = 0; frame_index < length; frame_index++)
{
frames[frame_index] = new TMOFrame(frame_index);
frames[frame_index].matrices = new TMOMat[frame1.matrices.Length];
}
for (int i = 0; i < frame1.matrices.Length; i++)
{
TMOMat[] interpolated_matrices = TMOMat.Slerp(
frame0.matrices[i],
frame1.matrices[i],
frame2.matrices[id_pair[i]],
frame3.matrices[id_pair[i]],
length,
p1);
for (int frame_index = 0; frame_index < length; frame_index++)
{
frames[frame_index].matrices[i] = interpolated_matrices[frame_index];
}
}
return(frames);
}
/// <summary>
/// 補間を行います。
/// </summary>
/// <param name="mat0">行列0</param>
/// <param name="mat1">行列1</param>
/// <param name="mat2">行列2</param>
/// <param name="mat3">行列3</param>
/// <param name="length">分割数</param>
/// <param name="p1">補間速度係数</param>
/// <returns>分割数だけTMOMatを持つ配列</returns>
public static TMOMat[] Slerp(TMOMat mat0, TMOMat mat1, TMOMat mat2, TMOMat mat3, int length, float p1)
{
TMOMat[] ret = new TMOMat[length];
Quaternion q1 = Quaternion.RotationMatrix(mat1.m);
Quaternion q2 = Quaternion.RotationMatrix(mat2.m);
Vector3 v0 = new Vector3(mat0.m.M41, mat0.m.M42, mat0.m.M43);
Vector3 v1 = new Vector3(mat1.m.M41, mat1.m.M42, mat1.m.M43);
Vector3 v2 = new Vector3(mat2.m.M41, mat2.m.M42, mat2.m.M43);
Vector3 v3 = new Vector3(mat3.m.M41, mat3.m.M42, mat3.m.M43);
float p0 = 0.0f;
float p2 = 1.0f;
float dt = 1.0f / length;
for (int i = 0; i < length; i++)
{
float t = dt * i;
float p = t * t * (p2 - 2 * p1 + p0) + t * (2 * p1 - 2 * p0) + p0;
Matrix m = Matrix.RotationQuaternion(Quaternion.Slerp(q1, q2, p)) * Matrix.Translation(Vector3.CatmullRom(v0, v1, v2, v3, p));
ret[i] = new TMOMat(ref m);
}
return(ret);
}
void Translate2(ref Matrix m, Matrix transformation)
{
Vector3 t = TMOMat.DecomposeMatrix(ref transformation);
m.M41 = t.X + m.M41;
m.M42 = t.Y + m.M42;
m.M43 = t.Z + m.M43;
}
private void LimitRotationZXY(TMONode node)
{
TMOConstraintItem item = constraint_zxy.GetItem(node.Name);
Vector3 angle1 = TMOMat.ToAngleZXY(node.Rotation);
Vector3 angle0 = item.Limit(angle1);
node.Rotation = TMOMat.ToQuaternionZXY(angle0);
//Console.WriteLine("node {0} x {1:F2} y {2:F2} z {3:F2}", node.Name, angle0.X, angle0.Y, angle0.Z);
}
public static void Write(BinaryWriter bw, TMOMat item)
{
Matrix m = item.m;
bw.Write(m.M11); bw.Write(m.M12); bw.Write(m.M13); bw.Write(m.M14);
bw.Write(m.M21); bw.Write(m.M22); bw.Write(m.M23); bw.Write(m.M24);
bw.Write(m.M31); bw.Write(m.M32); bw.Write(m.M33); bw.Write(m.M34);
bw.Write(m.M41); bw.Write(m.M42); bw.Write(m.M43); bw.Write(m.M44);
}
/// <summary>
/// 加減算の結果として新たなframeを得ます。
/// 新たなframeはframe0と同じnode並びとなります。
/// </summary>
/// <param name="frame0">frame0</param>
/// <param name="frame1">frame1</param>
/// <param name="frame2">frame2</param>
/// <param name="id_pair">node idのペア</param>
/// <returns>新たなframe</returns>
public static TMOFrame AddSub(TMOFrame frame0, TMOFrame frame1, TMOFrame frame2, int[] id_pair)
{
TMOFrame ret = new TMOFrame(0);
ret.matrices = new TMOMat[frame0.matrices.Length];
for (int i = 0; i < frame0.matrices.Length; i++)
{
ret.matrices[i] = TMOMat.AddSub(frame0.matrices[i], frame1.matrices[id_pair[i]], frame2.matrices[id_pair[i]]);
}
return(ret);
}
/// <summary>
/// 加減算を行います。
/// </summary>
/// <param name="mat0">行列0</param>
/// <param name="mat1">行列1</param>
/// <param name="mat2">行列2</param>
/// <returns>行列1 - 行列2 + 行列0</returns>
public static TMOMat AddSub(TMOMat mat0, TMOMat mat1, TMOMat mat2)
{
Matrix m0 = mat0.m;
Matrix m1 = mat1.m;
Matrix m2 = mat2.m;
Vector3 t0 = Helper.DecomposeMatrix(ref m0);
Vector3 t1 = Helper.DecomposeMatrix(ref m1);
Vector3 t2 = Helper.DecomposeMatrix(ref m2);
Matrix m = m1 * Matrix.Invert(m2) * m0 * Matrix.Translation(t1 - t2 + t0);
return(new TMOMat(ref m));
}
/// <summary>
/// モーション行列を指定比率で変形します。
/// </summary>
/// <param name="mat">モーション行列</param>
/// <param name="ratio">比率</param>
public void Transform(TMOMat mat, float ratio)
{
foreach (TPOCommand command in commands)
{
Matrix scaling = Matrix.Identity;
switch (command.type)
{
case TPOCommandType.Scale:
case TPOCommandType.Scale1:
case TPOCommandType.Scale0:
Vector3 v;
v.X = (float)Math.Pow(command.X, ratio);
v.Y = (float)Math.Pow(command.Y, ratio);
v.Z = (float)Math.Pow(command.Z, ratio);
scaling = Matrix.Scaling(v);
break;
}
switch (command.type)
{
case TPOCommandType.Scale:
mat.Scale(scaling);
break;
case TPOCommandType.Scale1:
mat.Scale1(scaling);
break;
case TPOCommandType.Scale0:
mat.Scale0(scaling);
break;
case TPOCommandType.Rotate:
mat.RotateX(command.X * ratio);
mat.RotateY(command.Y * ratio);
mat.RotateZ(command.Z * ratio);
break;
case TPOCommandType.Rotate2:
mat.RotateX2(command.X * ratio);
mat.RotateY2(command.Y * ratio);
mat.RotateZ2(command.Z * ratio);
break;
case TPOCommandType.Move:
mat.Move(command.GetVector3() * ratio);
break;
}
}
}
public static int DisplayTranslation(string source_file)
{
TMOFile tmo = new TMOFile();
try
{
tmo.Load(source_file);
}
catch (Exception ex)
{
Console.WriteLine(ex.Message);
return(-1);
}
if (tmo.nodes[0].ShortName != "W_Hips")
{
Console.WriteLine("Passed: root node is not W_Hips");
return(1);
}
Dictionary <string, TMONode> nodes = new Dictionary <string, TMONode>();
foreach (TMONode node in tmo.nodes)
{
try {
nodes.Add(node.ShortName, node);
} catch (ArgumentException) {
Console.WriteLine("node {0} already exists.", node.ShortName);
}
}
try {
TMONode node;
node = nodes["W_Hips"];
TMOMat mat = node.frame_matrices[0];
Matrix m = mat.m;
Console.WriteLine("{0} {1} {2}", m.M41, m.M42, m.M43);
} catch (KeyNotFoundException) {
Console.WriteLine("node not found.");
}
return(0);
}
public void Transform(TPONode node)
{
for (int i = 0; i < tmo.frames.Length; i++)
{
TMOMat mat = tmo.GetTMOMat(node.name, i);
Matrix m = mat.m;
Vector3 scaling = Vector3.Empty;
Vector3 t = TMOMat.DecomposeMatrix(ref m, out scaling);
scaling.X *= (float)Math.Pow(node.Scaling.X, ratio);
scaling.Y *= (float)Math.Pow(node.Scaling.Y, ratio);
scaling.Z *= (float)Math.Pow(node.Scaling.Z, ratio);
m *= node.RotationMatrix(m, ratio);
t += node.Translation * ratio;
mat.m = Matrix.Scaling(scaling) * m * Matrix.Translation(t);
}
}
public static TMOMat[] Slerp(TMOMat mat0, TMOMat mat1, TMOMat mat2, TMOMat mat3, int length)
{
TMOMat[] ret = new TMOMat[length];
Quaternion q1 = Quaternion.RotationMatrix(mat1.m);
Quaternion q2 = Quaternion.RotationMatrix(mat2.m);
Vector3 v0 = new Vector3(mat0.m.M41, mat0.m.M42, mat0.m.M43);
Vector3 v1 = new Vector3(mat1.m.M41, mat1.m.M42, mat1.m.M43);
Vector3 v2 = new Vector3(mat2.m.M41, mat2.m.M42, mat2.m.M43);
Vector3 v3 = new Vector3(mat3.m.M41, mat3.m.M42, mat3.m.M43);
float dt = 1.0f / length;
for (int i = 0; i < length; i++)
{
ret[i] = new TMOMat(Matrix.RotationQuaternion(Quaternion.Slerp(q1, q2, dt * i)) * Matrix.Translation(Vector3.CatmullRom(v0, v1, v2, v3, dt * i)));
}
return(ret);
}
/// <summary>
/// 指定番号のフレームに含まれるモーション行列値を変形します。
/// </summary>
/// <param name="frame_index">フレーム番号</param>
public void Transform(int frame_index)
{
if (ratio == 0)
{
return;
}
if (tmo.frames == null)
{
return;
}
TMOFrame frame = tmo.frames[frame_index];
Debug.Assert(frame.matrices.Length == nodes.Length);
for (int j = 0; j < frame.matrices.Length; j++)
{
TPONode node = nodes[j];
TMOMat mat = frame.matrices[j];
node.Transform(mat, ratio);
}
}
/// <summary>
/// モーション行列を指定比率で変形します。
/// </summary>
/// <param name="mat">モーション行列</param>
/// <param name="ratio">比率</param>
public void Transform(TMOMat mat, float ratio)
{
foreach (TPOCommand command in commands)
{
Matrix scaling = Matrix.Identity;
switch (command.type)
{
case TPOCommandType.Scale:
case TPOCommandType.Scale1:
case TPOCommandType.Scale0:
Vector3 v;
v.X = (float)Math.Pow(command.X, ratio);
v.Y = (float)Math.Pow(command.Y, ratio);
v.Z = (float)Math.Pow(command.Z, ratio);
scaling = Matrix.Scaling(v);
break;
}
switch (command.type)
{
case TPOCommandType.Scale:
mat.Scale(scaling);
break;
case TPOCommandType.Scale1:
mat.Scale1(scaling);
break;
case TPOCommandType.Scale0:
mat.Scale0(scaling);
break;
case TPOCommandType.Rotate:
mat.RotateX(command.X * ratio);
mat.RotateY(command.Y * ratio);
mat.RotateZ(command.Z * ratio);
break;
case TPOCommandType.Move:
mat.Move(command.GetVector3() * ratio);
break;
}
}
}
void Transform2(ref Matrix m, Matrix transformation)
{
Vector3 t = TMOMat.DecomposeMatrix(ref transformation);
m = transformation * m;
}
/// <summary>
/// 補間を行います。
/// </summary>
/// <param name="mat0">行列0</param>
/// <param name="mat1">行列1</param>
/// <param name="mat2">行列2</param>
/// <param name="mat3">行列3</param>
/// <param name="length">分割数</param>
/// <returns>分割数だけTMOMatを持つ配列</returns>
public static TMOMat[] Slerp(TMOMat mat0, TMOMat mat1, TMOMat mat2, TMOMat mat3, int length)
{
return(Slerp(mat0, mat1, mat2, mat3, length, 0.5f));
}
/// <summary>
/// 加減算を行います。
/// </summary>
/// <param name="mat0">行列0</param>
/// <param name="mat1">行列1</param>
/// <param name="mat2">行列2</param>
/// <returns>行列1 - 行列2 + 行列0</returns>
public static TMOMat AddSub(TMOMat mat0, TMOMat mat1, TMOMat mat2)
{
Matrix m0 = mat0.m;
Matrix m1 = mat1.m;
Matrix m2 = mat2.m;
Vector3 t0 = Helper.DecomposeMatrix(ref m0);
Vector3 t1 = Helper.DecomposeMatrix(ref m1);
Vector3 t2 = Helper.DecomposeMatrix(ref m2);
Matrix m = m1 * Matrix.Invert(m2) * m0 * Matrix.Translation(t1 - t2 + t0);
return new TMOMat(ref m);
}
/// <summary>
/// 補間を行います。
/// </summary>
/// <param name="mat0">行列0</param>
/// <param name="mat1">行列1</param>
/// <param name="mat2">行列2</param>
/// <param name="mat3">行列3</param>
/// <param name="length">分割数</param>
/// <returns>分割数だけTMOMatを持つ配列</returns>
public static TMOMat[] Slerp(TMOMat mat0, TMOMat mat1, TMOMat mat2, TMOMat mat3, int length)
{
return Slerp(mat0, mat1, mat2, mat3, length, 0.5f);
}
/// <summary>
/// 補間を行います。
/// </summary>
/// <param name="mat0">行列0</param>
/// <param name="mat1">行列1</param>
/// <param name="mat2">行列2</param>
/// <param name="mat3">行列3</param>
/// <param name="length">分割数</param>
/// <param name="p1">補間速度係数</param>
/// <returns>分割数だけTMOMatを持つ配列</returns>
public static TMOMat[] Slerp(TMOMat mat0, TMOMat mat1, TMOMat mat2, TMOMat mat3, int length, float p1)
{
TMOMat[] ret = new TMOMat[length];
Matrix m1 = mat1.m;
Matrix m2 = mat2.m;
Vector3 scaling1;
Vector3 scaling2;
Vector3 v1 = Helper.DecomposeMatrix(ref m1, out scaling1);
Vector3 v2 = Helper.DecomposeMatrix(ref m2, out scaling2);
Quaternion q1 = Quaternion.RotationMatrix(m1);
Quaternion q2 = Quaternion.RotationMatrix(m2);
Vector3 v0 = new Vector3(mat0.m.M41, mat0.m.M42, mat0.m.M43);
//Vector3 v1 = new Vector3(mat1.m.M41, mat1.m.M42, mat1.m.M43);
//Vector3 v2 = new Vector3(mat2.m.M41, mat2.m.M42, mat2.m.M43);
Vector3 v3 = new Vector3(mat3.m.M41, mat3.m.M42, mat3.m.M43);
float p0 = 0.0f;
float p2 = 1.0f;
float dt = 1.0f/length;
for (int i = 0; i < length; i++)
{
float t = dt*i;
float p = t*t*(p2-2*p1+p0) + t*(2*p1-2*p0) + p0;
Matrix m = Matrix.Scaling(Vector3.Lerp(scaling1, scaling2, p)) * Matrix.RotationQuaternion(Quaternion.Slerp(q1, q2, p)) * Matrix.Translation(Vector3.CatmullRom(v0, v1, v2, v3, p));
ret[i] = new TMOMat(ref m);
}
return ret;
}
/// <summary>
/// tmoファイルを含むディレクトリから要素を追加します。
/// </summary>
/// <param name="source_file">tmoファイルを含むディレクトリ名</param>
public void AddItemFromTMODirectory(string source_file)
{
TMOFile tmo = new TMOFile();
Dictionary <string, Vector3> min1_dic = new Dictionary <string, Vector3>();
Dictionary <string, Vector3> max1_dic = new Dictionary <string, Vector3>();
Dictionary <string, Vector3> min2_dic = new Dictionary <string, Vector3>();
Dictionary <string, Vector3> max2_dic = new Dictionary <string, Vector3>();
string[] files = Directory.GetFiles(source_file, "*.tmo");
foreach (string file in files)
{
tmo.Load(file);
foreach (TMONode node in tmo.nodes)
{
TMOMat mat = node.matrices[0];
string name = node.Name;
Vector3 angle1 = Helper.ToAngleXYZ(mat.m);
Vector3 angle2 = angle1;
if (angle2.X < 0)
{
angle2.X += 360;
}
if (angle2.Y < 0)
{
angle2.Y += 360;
}
if (angle2.Z < 0)
{
angle2.Z += 360;
}
if (!min1_dic.ContainsKey(name))
{
min1_dic[name] = new Vector3(+180.0f, +180.0f, +180.0f);
}
if (!max1_dic.ContainsKey(name))
{
max1_dic[name] = new Vector3(-180.0f, -180.0f, -180.0f);
}
Vector3 min1 = min1_dic[name];
Vector3 max1 = max1_dic[name];
if (angle1.X < min1_dic[name].X)
{
min1.X = angle1.X;
}
if (angle1.X > max1_dic[name].X)
{
max1.X = angle1.X;
}
if (angle1.Y < min1_dic[name].Y)
{
min1.Y = angle1.Y;
}
if (angle1.Y > max1_dic[name].Y)
{
max1.Y = angle1.Y;
}
if (angle1.Z < min1_dic[name].Z)
{
min1.Z = angle1.Z;
}
if (angle1.Z > max1_dic[name].Z)
{
max1.Z = angle1.Z;
}
min1_dic[name] = min1;
max1_dic[name] = max1;
if (!min2_dic.ContainsKey(name))
{
min2_dic[name] = new Vector3(360.0f, 360.0f, 360.0f);
}
if (!max2_dic.ContainsKey(name))
{
max2_dic[name] = new Vector3(0.0f, 0.0f, 0.0f);
}
Vector3 min2 = min2_dic[name];
Vector3 max2 = max2_dic[name];
if (angle2.X < min2_dic[name].X)
{
min2.X = angle2.X;
}
if (angle2.X > max2_dic[name].X)
{
max2.X = angle2.X;
}
if (angle2.Y < min2_dic[name].Y)
{
min2.Y = angle2.Y;
}
if (angle2.Y > max2_dic[name].Y)
{
max2.Y = angle2.Y;
}
if (angle2.Z < min2_dic[name].Z)
{
min2.Z = angle2.Z;
}
if (angle2.Z > max2_dic[name].Z)
{
max2.Z = angle2.Z;
}
min2_dic[name] = min2;
max2_dic[name] = max2;
}
}
foreach (string name in min1_dic.Keys)
{
TMOConstraintItem item = new TMOConstraintItem();
item.Name = name;
Vector3 min1 = min1_dic[name];
Vector3 max1 = max1_dic[name];
Vector3 min2 = min2_dic[name];
Vector3 max2 = max2_dic[name];
Vector3 sub1 = max1 - min1;
Vector3 sub2 = max2 - min2;
Vector3 min;
Vector3 max;
if (sub1.X <= sub2.X)
{
min.X = min1.X;
max.X = max1.X;
item.SectorX = 1;
}
else
{
min.X = min2.X;
max.X = max2.X;
item.SectorX = 2;
}
if (sub1.Y <= sub2.Y)
{
min.Y = min1.Y;
max.Y = max1.Y;
item.SectorY = 1;
}
else
{
min.Y = min2.Y;
max.Y = max2.Y;
item.SectorY = 2;
}
if (sub1.Z <= sub2.Z)
{
min.Z = min1.Z;
max.Z = max1.Z;
item.SectorZ = 1;
}
else
{
min.Z = min2.Z;
max.Z = max2.Z;
item.SectorZ = 2;
}
item.Min = min;
item.Max = max;
items.Add(item);
}
}
/// <summary>
/// 指定ライタに行列を書き出します。
/// </summary>
/// <param name="bw">ライタ</param>
/// <param name="item">行列</param>
public static void Write(BinaryWriter bw, TMOMat item)
{
Matrix m = item.m;
Write(bw, ref m);
}
