// Get the joint positions of the avatar in Unity by the forward kinematics
public List <Vector3> ForwardKinematics()
{
// get the joint positions, bone lengths and the joint local rotations
var positions = Positions.ToList();
var lengths = BoneLengths.ToList();
var rots = LocalRotations.ToList();
// get the joint order and the parent joint order(MPII order)
int[] joints = new int[] { 6, 2, 1, 0, 3, 4, 5, 7, 8, 9, 12, 11, 10, 13, 14, 15 };
int[] parentJoints = new int[] { 6, 6, 2, 1, 6, 3, 4, 6, 7, 8, 8, 12, 11, 8, 13, 14 }; // pelvis's parent is self(6)
// get the new joint positions of the avatar by FK
Vector3[] fkPositions = new Vector3[16];
Quaternion[] globalRotations = new Quaternion[16];
for (int i = 0; i < joints.Length; i++)
{
float len = lengths[i];
Quaternion localRot = rots[i];
int j = joints[i];
int pj = parentJoints[i];
Vector3 pPos = fkPositions[pj];
Quaternion parentRot = globalRotations[pj];
Quaternion globalRot = parentRot * localRot;
Vector3 translate = globalRot * Vector3.left * len;
Vector3 pos = pPos + translate;
fkPositions[j] = pos;
globalRotations[j] = globalRot;
}
// return the new joint positions of the avatar
return(fkPositions.Take(16).ToList());
}
// Clear the class buffer for BoneLengths/HumanToAvatarTransformRotations/Positions/Rotations/LocalRotations
public void Clear()
{
Positions.Clear();
HumanToAvatarTransformRotations.Clear();
BoneLengths.Clear();
Rotations.Clear();
LocalRotations.Clear();
}
public Wall[] GetValue(Wall[] walls)
{
var count = walls.Length;
var curRotation = Rotations[0];
var curLocalRotation = LocalRotations[0];
var prevRotation = Rotations[1];
var prevLocalRotation = LocalRotations[1];
var rotationEasingStore = new Vector3EasingStore
{
Easing = Easing,
StartVector = curRotation.Item1,
EndVector = Rotations[1].Item1
};
var localRotationEasingStore = new Vector3EasingStore
{
Easing = Easing,
StartVector = curLocalRotation.Item1,
EndVector = LocalRotations[1].Item1
};
for (var i = 0; i < count; i++)
{
var time = i / (count - 1f);
var nextRotation = Rotations.First(t => t.Item2 >= time);
if (nextRotation.Item2 > prevRotation.Item2)
{
curRotation = prevRotation;
rotationEasingStore.NextPoint(nextRotation.Item1);
}
var rotationTime = Math.Abs(time) <= 0 ? time : (time - curRotation.Item2) / (nextRotation.Item2 - curRotation.Item2);
var nextLocalRotation = LocalRotations.First(t => t.Item2 >= time);
if (nextLocalRotation.Item2 > prevLocalRotation.Item2)
{
curLocalRotation = prevLocalRotation;
localRotationEasingStore.NextPoint(nextLocalRotation.Item1);
}
var localRotationTime = Math.Abs(time) <= 0 ? time : (time - curLocalRotation.Item2) / (nextLocalRotation.Item2 - curLocalRotation.Item2);
walls[i].Rotation = rotationEasingStore.GetValue(rotationTime);
walls[i].LocalRotation = localRotationEasingStore.GetValue(localRotationTime);
if (Math.Abs(time) > 0)
{
prevRotation = nextRotation;
prevLocalRotation = nextLocalRotation;
}
}
return(walls);
}
// Clear the class buffer for Positions/AvatarBoneLengths/HumanBoneVectors/HumanBoneLengths/Rotations/LocalRotations/FKPositions
public void AllClear()
{
Positions.Clear();
AvatarBoneLengths.Clear();
HumanBoneVectors.Clear();
HumanBoneLengths.Clear();
Rotations.Clear();
LocalRotations.Clear();
FKPositions.Clear();
}
// Clear the class buffer for Positions/HumanToAvatarTransformRotations/AvatarBoneLengths/HumanBoneVectors/HumanBoneLengths/Rotations/LocalRotations/FKPositions
public void Clear()
{
Positions.Clear();
HumanToAvatarTransformRotations.Clear();
AvatarBoneLengths.Clear();
HumanBoneVectors.Clear();
HumanBoneLengths.Clear();
Rotations.Clear();
LocalRotations.Clear();
FKPositions.Clear();
}
// AllClear is called for clearing the class buffer for Positions/AvatarBoneLengths/Rotations/LocalRotations/HumanBoneVectors/HumanBoneLengths/FKPositions and Frame/UseAbsoluteCoordinate/UseLocalRotation/UseForwardKinematics/SetFootOnGround
public void AllClear()
{
Frame = 0;
Positions.Clear();
AvatarBoneLengths.Clear();
UseAbsoluteCoordinate = false;
UseLocalRotation = false;
UseForwardKinematics = false;
SetFootOnGround = false;
Rotations.Clear();
LocalRotations.Clear();
HumanBoneVectors.Clear();
HumanBoneLengths.Clear();
FKPositions.Clear();
}
public List <Vector3> FK()
{
var positions = Positions.ToList();
var hip = (positions[2] + positions[3]) * 0.5f;
positions.Add(hip);
var lengths = BoneLengths.ToList();
var hipLength = lengths[0] * 0.5f;
lengths[0] = hipLength;
lengths.Insert(0, -hipLength);
var rots = LocalRotations.ToList();
rots.Insert(0, rots[0]);
int[] joints = new int[] { 2, 3, 1, 0, 4, 5, 12, 8, 7, 6, 9, 10, 11, 13, };
// 14 means hip
int[] parentJoints = new int[] { 14, 14, 2, 1, 3, 4, 14, 12, 8, 7, 12, 9, 10, 12, };
Vector3[] fkPositions = new Vector3[15];
fkPositions[14] = hip;
Quaternion[] globalRotations = new Quaternion[15];
globalRotations[14] = Quaternion.identity;
for (int i = 0; i < joints.Length; i++)
{
float len = lengths[i];
Quaternion localRot = rots[i];
int j = joints[i];
int p = parentJoints[i];
Vector3 pPos = fkPositions[p];
Quaternion parentRot = globalRotations[p];
Quaternion globalRot = parentRot * localRot;
Vector3 translate = globalRot * Vector3.left * len;
Vector3 pos = pPos + translate;
fkPositions[j] = pos;
globalRotations[j] = globalRot;
}
return(fkPositions.Take(14).ToList());
}
// CalculateBoneRotations is called for calculating the bone rotations(global/local) in the method of joint angle mapping
public void CalculateBoneRotations()
{
// calculate the Bone Global Rotations
for (int i = 0; i < 15; i++)
{
Rotations.Add(Quaternion.identity); // initialize the Rotations
}
// check whether using absolute coordinate rotation and pick the different methods(aligning coordinates/relatively rotating) to calculate the bone rotations
if (UseAbsoluteCoordinate)
{
// calculate the bone global rotations using the method of aligning bone coordinates(make sure that the coordinate system will be the same(fixed to the bones) for every frame based on the human joint keypoints)
Rotations[0] = CalculateGlobalRotation(Positions[6] - Positions[2], Positions[7] - Positions[6]); //
Rotations[1] = CalculateGlobalRotation(Positions[2] - Positions[1], Positions[1] - Positions[0]); // rightThigh
Rotations[2] = CalculateGlobalRotation(Positions[1] - Positions[0], Positions[1] - Positions[2]); // rightCalf
Rotations[3] = CalculateGlobalRotation(Positions[6] - Positions[3], Positions[6] - Positions[7]); //
Rotations[4] = CalculateGlobalRotation(Positions[3] - Positions[4], Positions[4] - Positions[5]); // leftThigh
Rotations[5] = CalculateGlobalRotation(Positions[4] - Positions[5], Positions[4] - Positions[3]); // leftCalf
Rotations[6] = CalculateGlobalRotation(Positions[6] - Positions[7], Positions[3] - Positions[2]); //
Rotations[7] = CalculateGlobalRotation(Positions[7] - Positions[8], Positions[6] - Positions[7]); //
Rotations[8] = CalculateGlobalRotation(Positions[8] - Positions[9], Positions[7] - Positions[8]); //
Rotations[9] = CalculateGlobalRotation(Positions[8] - Positions[12], Positions[8] - Positions[7]); // rightClavicle
Rotations[10] = CalculateGlobalRotation(Positions[12] - Positions[11], Positions[11] - Positions[10]); // rightUpperArm
Rotations[11] = CalculateGlobalRotation(Positions[11] - Positions[10], Positions[11] - Positions[12]); // rightForearm
Rotations[12] = CalculateGlobalRotation(Positions[8] - Positions[13], Positions[7] - Positions[8]); //
Rotations[13] = CalculateGlobalRotation(Positions[13] - Positions[14], Positions[14] - Positions[15]); // leftUpperArm
Rotations[14] = CalculateGlobalRotation(Positions[14] - Positions[15], Positions[14] - Positions[13]); // leftForearm
}
else
{
// define the initial reference coordinate rotations
var rotLeftForward = Quaternion.LookRotation(Vector3.left, Vector3.forward);
var rotLeftBack = Quaternion.LookRotation(Vector3.left, Vector3.back);
var rotLeftDown = Quaternion.LookRotation(Vector3.left, Vector3.down);
var rotLeftUp = Quaternion.LookRotation(Vector3.left, Vector3.up);
// TODO: calculate the bone global rotations using the method of relatively rotating
Rotations[0] = CalculateGlobalRotation(Positions[2] - Positions[6], Positions[7] - Positions[6], rotLeftDown); // rightHipBone
Rotations[1] = CalculateGlobalRotation(Positions[1] - Positions[2], Positions[0] - Positions[1], rotLeftForward); // rightThigh
Rotations[2] = CalculateGlobalRotation(Positions[0] - Positions[1], Positions[2] - Positions[1], rotLeftForward); // rightCalf
Rotations[3] = CalculateGlobalRotation(Positions[3] - Positions[6], Positions[6] - Positions[7], rotLeftDown); // leftHipBone
Rotations[4] = CalculateGlobalRotation(Positions[4] - Positions[3], Positions[5] - Positions[4], rotLeftForward); // leftThigh
Rotations[5] = CalculateGlobalRotation(Positions[5] - Positions[4], Positions[3] - Positions[4], rotLeftForward); // leftCalf
Rotations[6] = CalculateGlobalRotation(Positions[7] - Positions[6], Positions[3] - Positions[2], rotLeftDown); // waist
Rotations[7] = CalculateGlobalRotation(Positions[8] - Positions[7], Positions[6] - Positions[7], rotLeftDown); // chest
Rotations[8] = CalculateGlobalRotation(Positions[9] - Positions[8], Positions[13] - Positions[12], rotLeftDown); // neck
Rotations[9] = CalculateGlobalRotation(Positions[12] - Positions[8], Positions[8] - Positions[7], rotLeftUp); // rightClavicle
Rotations[10] = CalculateGlobalRotation(Positions[11] - Positions[12], Positions[10] - Positions[11], rotLeftBack); // rightUpperArm
Rotations[11] = CalculateGlobalRotation(Positions[10] - Positions[11], Positions[12] - Positions[11], rotLeftBack); // rightForearm
Rotations[12] = CalculateGlobalRotation(Positions[13] - Positions[8], Positions[7] - Positions[8], rotLeftUp); // leftClavicle
Rotations[13] = CalculateGlobalRotation(Positions[14] - Positions[13], Positions[15] - Positions[14], rotLeftBack); // leftUpperArm
Rotations[14] = CalculateGlobalRotation(Positions[15] - Positions[14], Positions[13] - Positions[14], rotLeftBack); // leftForearm
}
// calculate the Bone Local Rotations
for (int j = 0; j < 15; j++)
{
LocalRotations.Add(Quaternion.identity); // initialize the LocalRotations
}
// check whether using local rotation of bones for driving avatar
if (UseLocalRotation)
{
// calculate the bone local rotations using the method of relatively rotating
LocalRotations[0] = CalculateLocalRotation(Rotations[6], Rotations[0]); // rightHipBone
LocalRotations[1] = CalculateLocalRotation(Rotations[0], Rotations[1]); // rightThigh
LocalRotations[2] = CalculateLocalRotation(Rotations[1], Rotations[2]); // rightCalf
LocalRotations[3] = CalculateLocalRotation(Rotations[6], Rotations[3]); // leftHipBone
LocalRotations[4] = CalculateLocalRotation(Rotations[3], Rotations[4]); // leftThigh
LocalRotations[5] = CalculateLocalRotation(Rotations[4], Rotations[5]); // leftCalf
LocalRotations[6] = Rotations[6]; // waist
LocalRotations[7] = CalculateLocalRotation(Rotations[6], Rotations[7]); // chest
LocalRotations[8] = CalculateLocalRotation(Rotations[7], Rotations[8]); // neck
LocalRotations[9] = CalculateLocalRotation(Rotations[8], Rotations[9]); // rightClavicle
LocalRotations[10] = CalculateLocalRotation(Rotations[9], Rotations[10]); // rightUpperArm
LocalRotations[11] = CalculateLocalRotation(Rotations[10], Rotations[11]); // rightForearm
LocalRotations[12] = CalculateLocalRotation(Rotations[8], Rotations[12]); // leftClavicle
LocalRotations[13] = CalculateLocalRotation(Rotations[12], Rotations[13]); // leftUpperArm
LocalRotations[14] = CalculateLocalRotation(Rotations[13], Rotations[14]); // leftForearm
}
}
// CalculateBoneRotations is called for calculating the bone rotations(global/local) in the method of joint angle mapping
public void CalculateBoneRotations()
{
// calculate the Bone Global Rotations
for (int i = 0; i < 16; i++)
{
Rotations.Add(Quaternion.identity); // initialize the Rotations
}
// check whether using absolute coordinate rotation and pick the different methods(aligning coordinates/relatively rotating) to calculate the bone rotations
if (UseAbsoluteCoordinate)
{
// calculate the bone global rotations using the method of aligning bone coordinates(make sure that the coordinate system will be the same(fixed to the bones) for every frame based on the human joint keypoints)
Rotations[0] = CalculateGlobalRotation(Positions[6] - Positions[7], Positions[3] - Positions[2], 3); // rightHipBone(rotTypeFlag:3)
Rotations[1] = CalculateGlobalRotation(Positions[2] - Positions[1], Positions[1] - Positions[0], 1); // rightThigh(rotTypeFlag:1)
Rotations[2] = CalculateGlobalRotation(Positions[1] - Positions[0], Positions[1] - Positions[2], 1); // rightCalf(rotTypeFlag:1)
Rotations[3] = CalculateGlobalRotation(Positions[6] - Positions[7], Positions[3] - Positions[2], 3); // leftHipBone(rotTypeFlag:3)
Rotations[4] = CalculateGlobalRotation(Positions[3] - Positions[4], Positions[4] - Positions[5], 1); // leftThigh(rotTypeFlag:1)
Rotations[5] = CalculateGlobalRotation(Positions[4] - Positions[5], Positions[4] - Positions[3], 1); // leftCalf(rotTypeFlag:1)
Rotations[6] = CalculateGlobalRotation(Positions[6] - Positions[7], Positions[3] - Positions[2], 3); // waist(rotTypeFlag:3)
Rotations[7] = CalculateGlobalRotation(Positions[7] - Positions[8], Positions[6] - Positions[7], 1); // chest(rotTypeFlag:1)
Rotations[8] = CalculateGlobalRotation(Positions[8] - Positions[9], Positions[7] - Positions[8], 1) * new Quaternion(0.0f, 0.0f, 0.342f, 0.940f); // neck(rotTypeFlag:1)(correct the neck up for 40 degrees in z axis rotation direction)
Rotations[9] = CalculateGlobalRotation(Positions[9] - Positions[10], Positions[8] - Positions[9], 1); // head(rotTypeFlag:1)
Rotations[10] = CalculateGlobalRotation(Positions[8] - Positions[13], Vector3.Cross(Positions[8] - Positions[13], Positions[8] - Positions[7]), 2); // rightClavicle(rotTypeFlag:2)
Rotations[11] = CalculateGlobalRotation(Positions[13] - Positions[12], Positions[12] - Positions[11], 1); // rightUpperArm(rotTypeFlag:1)
Rotations[12] = CalculateGlobalRotation(Positions[12] - Positions[11], Positions[12] - Positions[13], 1); // rightForearm(rotTypeFlag:1)
Rotations[13] = CalculateGlobalRotation(Positions[8] - Positions[14], Vector3.Cross(Positions[8] - Positions[14], Positions[7] - Positions[8]), 2); // leftClavicle(rotTypeFlag:2)
Rotations[14] = CalculateGlobalRotation(Positions[14] - Positions[15], Positions[15] - Positions[16], 1); // leftUpperArm(rotTypeFlag:1)
Rotations[15] = CalculateGlobalRotation(Positions[15] - Positions[16], Positions[15] - Positions[14], 1); // leftForearm(rotTypeFlag:1)
}
else
{
// define the initial reference coordinate rotations
var rotLeftForward = Quaternion.LookRotation(Vector3.left, Vector3.forward);
var rotLeftBack = Quaternion.LookRotation(Vector3.left, Vector3.back);
var rotLeftDown = Quaternion.LookRotation(Vector3.left, Vector3.down);
var rotLeftUp = Quaternion.LookRotation(Vector3.left, Vector3.up);
// calculate the bone global rotations using the method of relatively rotating
Rotations[0] = CalculateGlobalRotation(Positions[2] - Positions[6], Positions[7] - Positions[6], rotLeftDown); // rightHipBone
Rotations[1] = CalculateGlobalRotation(Positions[1] - Positions[2], Positions[0] - Positions[1], rotLeftForward); // rightThigh
Rotations[2] = CalculateGlobalRotation(Positions[0] - Positions[1], Positions[2] - Positions[1], rotLeftForward); // rightCalf
Rotations[3] = CalculateGlobalRotation(Positions[3] - Positions[6], Positions[6] - Positions[7], rotLeftDown); // leftHipBone
Rotations[4] = CalculateGlobalRotation(Positions[4] - Positions[3], Positions[5] - Positions[4], rotLeftForward); // leftThigh
Rotations[5] = CalculateGlobalRotation(Positions[5] - Positions[4], Positions[3] - Positions[4], rotLeftForward); // leftCalf
Rotations[6] = CalculateGlobalRotation(Positions[7] - Positions[6], Positions[3] - Positions[2], rotLeftDown); // waist
Rotations[7] = CalculateGlobalRotation(Positions[8] - Positions[7], Positions[6] - Positions[7], rotLeftDown); // chest
Rotations[8] = CalculateGlobalRotation(Positions[9] - Positions[8], Positions[13] - Positions[12], rotLeftDown); // neck
Rotations[9] = CalculateGlobalRotation(Positions[10] - Positions[9], Positions[8] - Positions[9], rotLeftDown); // head
Rotations[10] = CalculateGlobalRotation(Positions[13] - Positions[8], Positions[8] - Positions[7], rotLeftUp); // rightClavicle
Rotations[11] = CalculateGlobalRotation(Positions[12] - Positions[13], Positions[11] - Positions[12], rotLeftBack); // rightUpperArm
Rotations[12] = CalculateGlobalRotation(Positions[11] - Positions[12], Positions[13] - Positions[12], rotLeftBack); // rightForearm
Rotations[13] = CalculateGlobalRotation(Positions[14] - Positions[8], Positions[7] - Positions[8], rotLeftUp); // leftClavicle
Rotations[14] = CalculateGlobalRotation(Positions[15] - Positions[14], Positions[16] - Positions[15], rotLeftBack); // leftUpperArm
Rotations[15] = CalculateGlobalRotation(Positions[16] - Positions[15], Positions[14] - Positions[15], rotLeftBack); // leftForearm
}
// calculate the Bone Local Rotations
for (int j = 0; j < 16; j++)
{
LocalRotations.Add(Quaternion.identity); // initialize the LocalRotations
}
// check whether using local rotation of bones for driving avatar
if (UseLocalRotation)
{
// calculate the bone local rotations using the method of relatively rotating
LocalRotations[0] = CalculateLocalRotation(Rotations[6], Rotations[0]); // rightHipBone
LocalRotations[1] = CalculateLocalRotation(Rotations[0], Rotations[1]); // rightThigh
LocalRotations[2] = CalculateLocalRotation(Rotations[1], Rotations[2]); // rightCalf
LocalRotations[3] = CalculateLocalRotation(Rotations[6], Rotations[3]); // leftHipBone
LocalRotations[4] = CalculateLocalRotation(Rotations[3], Rotations[4]); // leftThigh
LocalRotations[5] = CalculateLocalRotation(Rotations[4], Rotations[5]); // leftCalf
// LocalRotations[6] = Rotations[6]; // waist
LocalRotations[6] = CalculateLocalRotation(new Quaternion(-0.5f, 0.5f, 0.5f, 0.5f), Rotations[6]); // waist(relative rotation of pelvis to the world coordinate system)
LocalRotations[7] = CalculateLocalRotation(Rotations[6], Rotations[7]); // chest
LocalRotations[8] = CalculateLocalRotation(Rotations[7], Rotations[8]); // neck
LocalRotations[9] = CalculateLocalRotation(Rotations[8], Rotations[9]); // head
LocalRotations[10] = CalculateLocalRotation(Rotations[8], Rotations[10]); // rightClavicle
LocalRotations[11] = CalculateLocalRotation(Rotations[10], Rotations[11]); // rightUpperArm
LocalRotations[12] = CalculateLocalRotation(Rotations[11], Rotations[12]); // rightForearm
LocalRotations[13] = CalculateLocalRotation(Rotations[8], Rotations[13]); // leftClavicle
LocalRotations[14] = CalculateLocalRotation(Rotations[13], Rotations[14]); // leftUpperArm
LocalRotations[15] = CalculateLocalRotation(Rotations[14], Rotations[15]); // leftForearm
}
}
