protected Pose currentPose()
{
// local states of each joint
List <Vector3> positions = new List <Vector3>();
List <Quaternion> rotations = new List <Quaternion>();
Queue <Transform> unvisited = new Queue <Transform>();
unvisited.Enqueue(transform.root);
while (unvisited.Count > 0)
{
Transform visiting = unvisited.Dequeue();
foreach (Transform child in visiting)
{
unvisited.Enqueue(child);
positions.Add(child.localPosition);
rotations.Add(child.localRotation);
}
}
Pose pose = new Pose();
for (int i = 0; i < positions.Count; i++)
{
pose.AddJoint(positions[i], rotations[i]);
}
return(pose);
}
public static Pose FromString(string str)
{
Pose pose = new Pose();
string[] components = str.Split(',');
Queue <string> componentQueue = new Queue <string>(components);
while (componentQueue.Count > 0)
{
float posX = float.Parse(componentQueue.Dequeue(),
CultureInfo.InvariantCulture);
float posY = float.Parse(componentQueue.Dequeue(),
CultureInfo.InvariantCulture);
float posZ = float.Parse(componentQueue.Dequeue(),
CultureInfo.InvariantCulture);
float rotX = float.Parse(componentQueue.Dequeue(),
CultureInfo.InvariantCulture);
float rotY = float.Parse(componentQueue.Dequeue(),
CultureInfo.InvariantCulture);
float rotZ = float.Parse(componentQueue.Dequeue(),
CultureInfo.InvariantCulture);
float rotW = float.Parse(componentQueue.Dequeue(),
CultureInfo.InvariantCulture);
Vector3 localPos = new Vector3(posX, posY, posZ);
Quaternion localRot = new Quaternion(rotX, rotY, rotZ, rotW);
pose.AddJoint(localPos, localRot);
}
return(pose);
}
//--------------------------------------------------------------------------HELPERS:
protected Pose blendPoses(Pose a, Pose b, float t)
{
Pose blendedPose = new Pose();
int numJoints = a.NumJoints;
for (int i = 0; i < numJoints; i++)
{
Vector3 pos = Vector3.Lerp(a.GetJointPos(i),
b.GetJointPos(i),
t);
Quaternion rot = Quaternion.Slerp(a.GetJointRot(i),
b.GetJointRot(i),
t);
blendedPose.AddJoint(pos, rot);
}
return(blendedPose);
}
