/// <summary>
/// 迭代得到转化坐标系
/// </summary>
/// <param name="name">关节名称</param>
/// <param name="motpos">动作</param>
/// <param name="anipos">经过转化的动作</param>
protected void TransformBone(string name, Motion.Posture motpos, Posture anipos)
{
Skeleton.Bone skebone = Skeleton.Bones[name];
Motion.Bone motbone;
#region 判断是否存在节点
bool bonecontains = false;
foreach (string key in motpos.Bones.Keys)
{
if (key.Equals(name))
{
bonecontains = true;
break;
}
}
if (bonecontains)
{
motbone = motpos.Bones[name];
}
else
{
motbone = new Motion.Bone();
}
#endregion
Bone anibone = new Bone();
if ("root" == name)
{
if ("root" == CoordPara)
{
anibone.WorldTransform.MakeTransform(Vector3.ZERO, Vector3.UNIT_SCALE, Quaternion.IDENTITY);
}
else if ("global" == CoordPara)
{
Vector3 vec = new Vector3();
vec.x = motbone.DofVals[0];
vec.y = motbone.DofVals[1];
vec.z = motbone.DofVals[2];
Matrix3 mx = new Matrix3();
mx.FromAxisAngle(Vector3.UNIT_X, motbone.DofVals[3]);
Matrix3 my = new Matrix3();
my.FromAxisAngle(Vector3.UNIT_Y, motbone.DofVals[4]);
Matrix3 mz = new Matrix3();
mz.FromAxisAngle(Vector3.UNIT_Z, motbone.DofVals[5]);
Quaternion localorient = new Quaternion(mz * my * mx);
anibone.WorldTransform.MakeTransform(vec, Vector3.UNIT_SCALE, localorient);
}
anibone.WorldTranslate = anibone.WorldTransform.GetTrans();
anibone.WorldOrient = anibone.WorldTransform.ExtractQuaternion();
//anibone.WorldTranslateEnd = anibone.WorldTranslate;
}
else
{
Vector3 dof = Vector3.ZERO;
for (int ind = 0; ind < skebone.DofNames.Count; ++ind)
{
string dofname = skebone.DofNames[ind];
float val = motbone.DofVals[ind];
if ("rx" == dofname)
{
dof.x = val;
}
else if ("ry" == dofname)
{
dof.y = val;
}
else if ("rz" == dofname)
{
dof.z = val;
}
}
Matrix3 mx = new Matrix3();
mx.FromAxisAngle(Vector3.UNIT_X, dof.x);
Matrix3 my = new Matrix3();
my.FromAxisAngle(Vector3.UNIT_Y, dof.y);
Matrix3 mz = new Matrix3();
mz.FromAxisAngle(Vector3.UNIT_Z, dof.z);
Quaternion localorient = new Quaternion(mz * my * mx);
string parentname = skebone.Parent.Name;
Bone parentanibone = anipos.Bones[parentname];
Skeleton.Bone parentskebone = skebone.Parent;
Matrix4 parenttransform = new Matrix4();
parenttransform.MakeTransform(
parentskebone.Dir * parentskebone.Length,
Vector3.UNIT_SCALE,
skebone.ParentOrient * localorient
);
anibone.WorldTransform = parentanibone.WorldTransform * parenttransform;
anibone.WorldTranslate = anibone.WorldTransform.GetTrans();
anibone.WorldOrient = anibone.WorldTransform.ExtractQuaternion() * Vector3.UNIT_Y.GetRotationTo(skebone.Dir);
// anibone.WorldTranslateEnd = anibone.WorldTranslate + anibone.WorldOrient * new Vector3(0, skebone.Length, 0);
}
anipos.Bones.Add(name, anibone);
List <Skeleton.Bone> children = skebone.Children;
if (children != null)
{
for (int ind = 0; ind < children.Count; ++ind)
{
TransformBone(children[ind].Name, motpos, anipos);
}
}
}
/// <summary>
/// 根据文件建立骨架
/// </summary>
/// <param name="filename">asf文件的绝对路径</param>
/// <returns>Skeleton 对象</returns>
public static Skeleton CreateSkeletonFromAsfFile(string filename)
{
AsfParse gap = new AsfParse(filename);
gap.Parse();
Skeleton ske = new Skeleton();
Bone root = new Bone();
root.Name = "root";
root.DofNames.Add("tx"); root.DofNames.Add("ty"); root.DofNames.Add("ty");
root.DofNames.Add("rx"); root.DofNames.Add("ry"); root.DofNames.Add("rz");
ske.Bones.Add(root.Name, root);
// 关节信息
int numbone = gap.Bones.Count;
for (int indbone = 0; indbone < numbone; ++indbone)
{
Bone bone = new Bone();
bone.Name = gap.Bones[indbone].Name;
bone.Length = float.Parse(gap.Bones[indbone].Length);
bone.Dir.x = float.Parse(gap.Bones[indbone].Direction[0]);
bone.Dir.y = float.Parse(gap.Bones[indbone].Direction[1]);
bone.Dir.z = float.Parse(gap.Bones[indbone].Direction[2]);
bone.Axis.x = float.Parse(gap.Bones[indbone].Axis[0]);
bone.Axis.y = float.Parse(gap.Bones[indbone].Axis[1]);
bone.Axis.z = float.Parse(gap.Bones[indbone].Axis[2]);
int numdof = 0;
if (gap.Bones[indbone].Dofs != null)
{
numdof = gap.Bones[indbone].Dofs.Length;
}
for (int inddof = 0; inddof < numdof; ++inddof)
{
string namedof = gap.Bones[indbone].Dofs[inddof];
bone.DofNames.Add(namedof);
}
ske.Bones.Add(bone.Name, bone.Clone());
}
// -- 关节关联
int numhier = gap.HierarchyItems.Count;
for (int indhier = 0; indhier < numhier; ++indhier)
{
string bonename = gap.HierarchyItems[indhier].Parent;
int numchildren = gap.HierarchyItems[indhier].Children.Count;
for (int indchild = 0; indchild < numchildren; ++indchild)
{
string childname = gap.HierarchyItems[indhier].Children[indchild];
Skeleton.Bone parent = ske.Bones[bonename];
Skeleton.Bone child = ske.Bones[childname];
if (parent.Children == null)
{
parent.Children = new List <Bone>();
}
parent.Children.Add(child);
child.Parent = parent;
Matrix3 childmx = new Matrix3();
Matrix3 childmy = new Matrix3();
Matrix3 childmz = new Matrix3();
Matrix3 parentinvmx = new Matrix3();
Matrix3 parentinvmy = new Matrix3();
Matrix3 parentinvmz = new Matrix3();
childmx.FromAxisAngle(Vector3.UNIT_X, child.Axis.x);//子节点：从局部坐标系到全局坐标系
childmy.FromAxisAngle(Vector3.UNIT_Y, child.Axis.y);
childmz.FromAxisAngle(Vector3.UNIT_Z, child.Axis.z);
parentinvmx.FromAxisAngle(Vector3.UNIT_X, -parent.Axis.x); //父节点：从全局坐标系到局部坐标系
parentinvmy.FromAxisAngle(Vector3.UNIT_Y, -parent.Axis.y);
parentinvmz.FromAxisAngle(Vector3.UNIT_Z, -parent.Axis.z);
child.ParentOrient.FromRotationMatrix(parentinvmx * parentinvmy * parentinvmz * childmz * childmy * childmx);
Matrix3 childinvmx = new Matrix3();
Matrix3 childinvmy = new Matrix3();
Matrix3 childinvmz = new Matrix3();
childinvmx.FromAxisAngle(Vector3.UNIT_X, -child.Axis.x);//子节点：从全局坐标系到局部坐标系
childinvmy.FromAxisAngle(Vector3.UNIT_Y, -child.Axis.y);
childinvmz.FromAxisAngle(Vector3.UNIT_Z, -child.Axis.z);
child.Dir = childinvmx * childinvmy * childinvmz * child.Dir;
child.Dir.Normalize();
}
}
return(ske);
}