/// <summary>
/// Cyclic-Coordinate-Descent (CCD) 法による逆運動学の実装です。
/// </summary>
/// <param name="effector">エフェクタnode</param>
/// <param name="node">対象node</param>
/// <param name="target">目標</param>
public bool SolveIK(PmdNode effector, PmdNode node, Vector3 target)
{
Vector3 worldTargetP = target;
Vector3 worldEffectorP = effector.GetWorldPosition();
//Vector3 worldNodeP = node.GetWorldPosition();
Matrix invCoord = Matrix.Invert(node.GetWorldCoordinate());
Vector3 localEffectorP = Vector3.TransformCoordinate(worldEffectorP, invCoord);
Vector3 localTargetP = Vector3.TransformCoordinate(worldTargetP, invCoord);
Quaternion q;
if (RotationVectorToVector(localEffectorP, localTargetP, out q))
{
if (node.IsKnee)
{
Vector3 angle = ToAngleXYZ(q);
angle.X = Clamp(angle.X, -(float)Math.PI, -0.002f);
angle.Y = 0;
angle.Z = 0;
q = ToQuaternionXYZ(angle);
}
node.Rotation = q * node.Rotation;
}
return((localEffectorP - localTargetP).LengthSq() < 0.1f);
}
/// <summary>
/// ワールド座標系での位置と向きを得ます。
/// </summary>
/// <returns></returns>
public Matrix GetWorldCoordinate()
{
PmdNode node = this;
Matrix m = Matrix.Identity;
while (node != null)
{
m.Multiply(node.TransformationMatrix);
node = node.parent;
}
return(m);
}
/// <summary>
/// ワールド座標系での位置を得ます。
/// </summary>
/// <returns></returns>
public Vector3 GetWorldPosition()
{
PmdNode node = this;
Vector3 v = Vector3.Empty;
while (node != null)
{
v = Vector3.TransformCoordinate(v, node.TransformationMatrix);
node = node.parent;
}
return(v);
}
/// <summary>
/// bone行列を更新します。
/// </summary>
public void UpdateBoneMatrices(PmdNode node)
{
matrixStack.Push();
Matrix m = Matrix.RotationQuaternion(node.Rotation) * Matrix.Translation(node.Translation);
matrixStack.MultiplyMatrixLocal(m);
node.combined_matrix = matrixStack.Top;
foreach (PmdNode child_node in node.children)
{
UpdateBoneMatrices(child_node);
}
matrixStack.Pop();
}
public void Load(Stream source_stream)
{
BinaryReader reader = new BinaryReader(source_stream, System.Text.Encoding.Default);
byte[] magic = reader.ReadBytes(3);
if (magic[0] != (byte)'P' || magic[1] != (byte)'m' || magic[2] != (byte)'d')
{
throw new Exception("File is not Pmd");
}
float version = reader.ReadSingle();
Debug.WriteLine("version:" + version);
string model_name = reader.ReadCString(20);
Debug.WriteLine("model_name:" + model_name);
string comment = reader.ReadCString(256);
Debug.WriteLine("comment:" + comment);
int vertex_count = reader.ReadInt32();
Debug.WriteLine("vertex_count:" + vertex_count);
vertices = new Vertex[vertex_count];
for (int i = 0; i < vertex_count; i++)
{
vertices[i] = new Vertex();
vertices[i].Read(reader);
}
int face_vertex_count = reader.ReadInt32();
Debug.WriteLine("face_vertex_count:" + face_vertex_count);
indices = new ushort[face_vertex_count];
for (int i = 0; i < face_vertex_count; i++)
{
indices[i] = reader.ReadUInt16();
}
int face_count = face_vertex_count / 3;
Debug.WriteLine("face_count:" + face_count);
int material_count = reader.ReadInt32();
Debug.WriteLine("material_count:" + material_count);
materials = new PmdMaterial[material_count];
for (int i = 0; i < material_count; i++)
{
materials[i] = new PmdMaterial(i);
materials[i].Read(reader);
}
int face_vertex_start = 0;
for (int i = 0; i < material_count; i++)
{
face_vertex_start = materials[i].InjectFaceVertexStart(face_vertex_start);
}
ushort node_count = reader.ReadUInt16();
Debug.WriteLine("node_count:" + node_count);
nodes = new PmdNode[node_count];
for (ushort i = 0; i < node_count; i++)
{
nodes[i] = new PmdNode(i);
nodes[i].Read(reader);
}
for (ushort i = 0; i < node_count; i++)
{
nodes[i].ComputeOffsetMatrix();
}
GenerateNodemapAndTree();
bone_matrices = new Matrix[node_count];
ushort ik_count = reader.ReadUInt16();
Debug.WriteLine("ik_count:" + ik_count);
iks = new PmdIK[ik_count];
for (ushort i = 0; i < ik_count; i++)
{
iks[i] = new PmdIK();
iks[i].Read(reader);
}
UpdateNodeIKs();
ushort skin_count = reader.ReadUInt16();
Debug.WriteLine("skin_count:" + skin_count);
skins = new PmdSkin[skin_count];
for (ushort i = 0; i < skin_count; i++)
{
skins[i] = new PmdSkin();
skins[i].Read(reader);
}
for (ushort i = 1; i < skin_count; i++)
{
skins[i].Absolutize(skins[0]);
}
}
