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); }