public static Vector3 GetVecWithoutYRot(Vector3 coords)
{
SphericalCoords sphCoords = CartesianToSpherical(coords);
sphCoords.theta = 0;
return(sphCoords.ToCartesian());
}
private void FillKNNTrees(KNNBone rootBone, bool ignoreRotation, bool bothHandsInFeatureVec, string headBoneName, string rHandBoneName, string lHandBoneName, float slidingWindowSizeInMS, float slidingWindowOffsetInMS, float pollingRate, List <int> lHandTreeNodes, List <int> rHandTreeNodes, List <float[]> lHandTreePoints, List <float[]> rHandTreePoints)
{
float sourceFrameTime = bvhParser.frameTime;
float targetFrameTime = 1.0f / pollingRate;
float totalTime = bvhParser.frames * sourceFrameTime;
int totalNewFrames = (int)(totalTime / targetFrameTime);
float slidingWindowOffset = slidingWindowOffsetInMS / 1000.0f;
float slidingWindowSize = slidingWindowSizeInMS / 1000.0f;
int totalWindows = (int)((totalTime - (slidingWindowSize)) / (slidingWindowOffset));
int framesPerWindow = (int)((slidingWindowSize) / targetFrameTime);
int nrOfFeatures = bothHandsInFeatureVec ? 18 : 9;
int featureVecLength = nrOfFeatures * framesPerWindow;
setToFrame(0, true, true, true);
for (int currentWindow = 0; currentWindow < totalWindows; currentWindow++)
{
float[] lHandPoints = new float[featureVecLength];
float[] rHandPoints = new float[featureVecLength];
for (int currFrame = 0; currFrame < framesPerWindow; currFrame++)
{
float startTime = slidingWindowOffset * (float)currentWindow;
float sourceFrame = getSourceFrameFromTargetTime(sourceFrameTime, startTime + (float)currFrame * targetFrameTime);
setToFrame(sourceFrame, true, true, true);
Vector3 rHandPosition = root.Find(rHandBoneName).position;
Vector3 lHandPosition = root.Find(lHandBoneName).position;
if (sourceFrame >= 1.0f)
{
setToFrame(sourceFrame - 1.0f, true, true, true);
}
Vector3 prevRHandPosition = root.Find(rHandBoneName).position;
Vector3 prevLHandPosition = root.Find(lHandBoneName).position;
if (sourceFrame >= 2.0f)
{
setToFrame(sourceFrame - 2.0f, true, true, true);
}
Vector3 prevPrevRHandPosition = root.Find(rHandBoneName).position;
Vector3 prevPrevLHandPosition = root.Find(lHandBoneName).position;
Vector3 lHandVelocity = (lHandPosition - prevLHandPosition) / sourceFrameTime;
Vector3 rHandVelocity = (rHandPosition - prevRHandPosition) / sourceFrameTime;
Vector3 prevLHandVelocity = (prevLHandPosition - prevPrevLHandPosition) / sourceFrameTime;
Vector3 prevRHandVelocity = (prevRHandPosition - prevPrevRHandPosition) / sourceFrameTime;
Vector3 lHandAcceleration = (lHandVelocity - prevLHandVelocity) / sourceFrameTime;
Vector3 rHandAcceleration = (rHandVelocity - prevRHandVelocity) / sourceFrameTime;
if (ignoreRotation)
{
SphericalCoords rHandSpPosition = SphericalCoords.CartesianToSpherical(rHandPosition);
SphericalCoords lHandSpPosition = SphericalCoords.CartesianToSpherical(lHandPosition);
float rHandAngle = rHandSpPosition.theta * Mathf.Rad2Deg;
float lHandAngle = lHandSpPosition.theta * Mathf.Rad2Deg;
//rotate velocity and acceleration angle around "theta"
lHandVelocity = Quaternion.AngleAxis(lHandAngle, Vector3.up) * lHandVelocity;
rHandVelocity = Quaternion.AngleAxis(rHandAngle, Vector3.up) * rHandVelocity;
lHandAcceleration = Quaternion.AngleAxis(lHandAngle, Vector3.up) * lHandAcceleration;
rHandAcceleration = Quaternion.AngleAxis(rHandAngle, Vector3.up) * rHandAcceleration;
rHandSpPosition.theta = 0;
lHandSpPosition.theta = 0;
rHandPosition = rHandSpPosition.ToCartesian();
lHandPosition = lHandSpPosition.ToCartesian();
}
int pointOffset = currFrame * nrOfFeatures;
lHandPoints[pointOffset] = lHandPosition.x;
lHandPoints[pointOffset + 1] = lHandPosition.y;
lHandPoints[pointOffset + 2] = lHandPosition.z;
lHandPoints[pointOffset + 3] = lHandVelocity.x;
lHandPoints[pointOffset + 4] = lHandVelocity.y;
lHandPoints[pointOffset + 5] = lHandVelocity.z;
lHandPoints[pointOffset + 6] = lHandAcceleration.x;
lHandPoints[pointOffset + 7] = lHandAcceleration.y;
lHandPoints[pointOffset + 8] = lHandAcceleration.z;
rHandPoints[pointOffset] = rHandPosition.x;
rHandPoints[pointOffset + 1] = rHandPosition.y;
rHandPoints[pointOffset + 2] = rHandPosition.z;
rHandPoints[pointOffset + 3] = rHandVelocity.x;
rHandPoints[pointOffset + 4] = rHandVelocity.y;
rHandPoints[pointOffset + 5] = rHandVelocity.z;
rHandPoints[pointOffset + 6] = rHandAcceleration.x;
rHandPoints[pointOffset + 7] = rHandAcceleration.y;
rHandPoints[pointOffset + 8] = rHandAcceleration.z;
if (bothHandsInFeatureVec)
{
lHandPoints[pointOffset + 9] = rHandPosition.x;
lHandPoints[pointOffset + 10] = rHandPosition.y;
lHandPoints[pointOffset + 11] = rHandPosition.z;
lHandPoints[pointOffset + 12] = rHandVelocity.x;
lHandPoints[pointOffset + 13] = rHandVelocity.y;
lHandPoints[pointOffset + 14] = rHandVelocity.z;
lHandPoints[pointOffset + 15] = rHandAcceleration.x;
lHandPoints[pointOffset + 16] = rHandAcceleration.y;
lHandPoints[pointOffset + 17] = rHandAcceleration.z;
rHandPoints[pointOffset + 9] = lHandPosition.x;
rHandPoints[pointOffset + 10] = lHandPosition.y;
rHandPoints[pointOffset + 11] = lHandPosition.z;
rHandPoints[pointOffset + 12] = lHandVelocity.x;
rHandPoints[pointOffset + 13] = lHandVelocity.y;
rHandPoints[pointOffset + 14] = lHandVelocity.z;
rHandPoints[pointOffset + 15] = lHandAcceleration.x;
rHandPoints[pointOffset + 16] = lHandAcceleration.y;
rHandPoints[pointOffset + 17] = lHandAcceleration.z;
}
}
int lastPositionSourceIndex = (int)((currentWindow * slidingWindowOffset + slidingWindowSize) / sourceFrameTime);
setToFrame(lastPositionSourceIndex, true, true, true);
rootBone.AddRotationsFromTransform(root);
lHandTreePoints.Add(lHandPoints); lHandTreeNodes.Add(lHandTreeNodes.Count);
rHandTreePoints.Add(rHandPoints); rHandTreeNodes.Add(rHandTreeNodes.Count);
}
}
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);
}