private static Vector3 ToRotationIndex(Vector3 v, RotationIndex targetRotation)
{
switch (targetRotation)
{
// Z = positive Z axis
case RotationIndex.PosXPosYPosZ:
break;
case RotationIndex.PosYNegXPosZ:
return(new Vector3(v.Y, -v.X, v.Z));
case RotationIndex.NegXNegYPosZ:
return(new Vector3(-v.X, -v.Y, v.Z));
case RotationIndex.NegYPosXPosZ:
return(new Vector3(-v.Y, v.X, v.Z));
// Z = positive Y axis
case RotationIndex.PosXNegZPosY:
return(new Vector3(v.X, -v.Z, v.Y));
case RotationIndex.NegZNegXPosY:
return(new Vector3(-v.Z, -v.X, v.Y));
case RotationIndex.NegXPosZPosY:
return(new Vector3(-v.X, v.Z, v.Y));
case RotationIndex.PosZPosXPosY:
return(new Vector3(v.Z, v.X, v.Y));
// Z = negative Z axis
case RotationIndex.PosXNegYNegZ:
return(new Vector3(v.X, -v.Y, -v.Z));
case RotationIndex.NegYNegXNegZ:
return(new Vector3(-v.Y, -v.X, -v.Z));
case RotationIndex.NegXPosYNegZ:
return(new Vector3(-v.X, v.Y, -v.Z));
case RotationIndex.PosYPosXNegZ:
return(new Vector3(v.Y, v.X, -v.Z));
// Z = negative Y axis
case RotationIndex.PosXPosZNegY:
return(new Vector3(v.X, v.Z, -v.Y));
case RotationIndex.PosZNegXNegY:
return(new Vector3(v.Z, -v.X, -v.Y));
case RotationIndex.NegXNegZNegY:
return(new Vector3(-v.X, -v.Z, -v.Y));
case RotationIndex.NegZPosXNegY:
return(new Vector3(-v.Z, v.X, -v.Y));
// Z = positive X axis
case RotationIndex.NegZPosYPosX:
return(new Vector3(-v.Z, v.Y, v.X));
case RotationIndex.PosYPosZPosX:
return(new Vector3(v.Y, v.Z, v.X));
case RotationIndex.PosZNegYPosX:
return(new Vector3(v.Z, -v.Y, v.X));
case RotationIndex.NegYNegZPosX:
return(new Vector3(-v.Y, -v.Z, v.X));
// Z = negative X axis
case RotationIndex.PosZPosYNegX:
return(new Vector3(v.Z, v.Y, -v.X));
case RotationIndex.PosYNegZNegX:
return(new Vector3(v.Y, -v.Z, -v.X));
case RotationIndex.NegZNegYNegX:
return(new Vector3(-v.Z, -v.Y, -v.X));
case RotationIndex.NegYPosZNegX:
return(new Vector3(-v.Y, v.Z, -v.X));
}
return(v);
}
public void SetSkeletonFromHandPos(KDTree <float, int> tree, LinkedList <FeatureItem> features, string handName, int k)
{
float[] featureVec = new float[featureVectorLength];
Transform headTransform = rootBone.transform.Find("LowerBack/Spine/Spine1/Neck/Neck1/Head");
Transform neckTransform = rootBone.transform.Find("LowerBack/Spine/Spine1/Neck/Neck1/Head");
rootBone.transform.position -= headTransform.position;
float handTargetSpPositionAngle = SphericalCoords.GetYRotFromVec(features.Last().position) * Mathf.Rad2Deg;
var currFeatureContainer = features.Last;
Debug.Assert(featureVectorLength % 9 == 0);
for (int i = 0; i < featureVectorLength / 9; i++)
{
FeatureItem currFeatureItem = currFeatureContainer.Value;
Vector3 handPos = currFeatureItem.position;
Vector3 handVel = currFeatureItem.velocity;
Vector3 handAcc = currFeatureItem.acceleration;
if (ignoreRotation)
{
float handYRotValue = SphericalCoords.GetYRotFromVec(handPos) * Mathf.Rad2Deg;
SphericalCoords sphCoords = SphericalCoords.CartesianToSpherical(handPos);
sphCoords.theta = 0;
Vector3 outVec = sphCoords.ToCartesian();
handPos = Quaternion.AngleAxis(handYRotValue, Vector3.up) * handPos;
handVel = Quaternion.AngleAxis(handYRotValue, Vector3.up) * handVel;
handAcc = Quaternion.AngleAxis(handYRotValue, Vector3.up) * handAcc;
}
int startIndex = 9 * i;
featureVec[startIndex] = handPos.x;
featureVec[startIndex + 1] = handPos.y;
featureVec[startIndex + 2] = handPos.z;
featureVec[startIndex + 3] = handVel.x;
featureVec[startIndex + 4] = handVel.y;
featureVec[startIndex + 5] = handVel.z;
featureVec[startIndex + 6] = handAcc.x;
featureVec[startIndex + 7] = handAcc.y;
featureVec[startIndex + 8] = handAcc.z;
if (currFeatureContainer.Previous != null)
{
currFeatureContainer = currFeatureContainer.Previous;
}
}
Tuple <float[], int>[] poseIndices = tree.NearestNeighbors(featureVec, k);
int index = poseIndices[0].Item2;
RotationIndex[] rotations = new RotationIndex[poseIndices.Length];
for (int i = 0; i < poseIndices.Length; i++)
{
double distance = Metrics.WeightedL2Norm(poseIndices[i].Item1, featureVec);
RotationIndex currIdx = new RotationIndex(poseIndices[i].Item2, (float)(1.0 / distance));
rotations[i] = currIdx;
}
rootBone.SetToRotations(rotations);
//rootBone.SetToRotation(index);
Transform handTransform = rootBone.transform.Find(handName);
SphericalCoords handTransformSpPosition = SphericalCoords.CartesianToSpherical(handTransform.position - headTransform.position);
float handTransformSpPositionAngle = handTransformSpPosition.theta * Mathf.Rad2Deg;
rootBone.transform.localRotation *= Quaternion.Euler(0, -handTargetSpPositionAngle + handTransformSpPositionAngle, 0);
}