public void recordData(float t, Data.X_POS xPos, Data.Y_POS yPos, bool continuous)
{
index++;
int i = index % RECORD_FRAMS;
timestamp[i] = t;
if (continuous && index - 1 >= 0)
{
xPos.checkRotation(xPosList[(index - 1) % RECORD_FRAMS]);
yPos.checkRotation(yPosList[(index - 1) % RECORD_FRAMS]);
}
xPosList[i] = xPos;
yPosList[i] = yPos;
if (index - 1 >= 0)
{
float timeInterval = timestamp[i] - timestamp[(index - 1) % RECORD_FRAMS];
xSpeedList[i] = new Data.X_POS((xPosList[i] - xPosList[(index - 1) % RECORD_FRAMS]) / timeInterval);
ySpeedList[i] = new Data.Y_POS((yPosList[i] - yPosList[(index - 1) % RECORD_FRAMS]) / timeInterval);
}
else
{
xSpeedList[i] = new Data.X_POS();
ySpeedList[i] = new Data.Y_POS();
}
xPosSmooth[i] = xRollingMean(xPosList);
yPosSmooth[i] = yRollingMean(yPosList);
xSpeedSmooth[i] = xRollingMean(xSpeedList);
ySpeedSmooth[i] = yRollingMean(ySpeedList);
}
protected void retrieval()
{
if (movingDetect.isMoving() || currMotion == "walking")
{
if (movingDetect.isFirstMove())
{
resetCaliMotions();
}
float minScore = 1e9f;
float predictFrame = 1f;
foreach (string name in motionName)
{
for (int i = 0; i < CALI_NUM; i++)
{
float score = 0f;
float frame = caliMotions[name][i].predictMotionFrame(record, out score);
if (currMotion == name && score < minScore)
{
minScore = score;
predictFrame = frame / caliMotions[name][i].timestamp.Count * stdMotions[currMotion].timestamp.Count;
}
if (name == "walking" || name == "running")
{
break;
}
}
}
Data.Y_POS predictYPos = stdMotions[currMotion].getYPos(predictFrame);
setLowerBody(new Data.Y_POS(predictYPos + stdMotions[currMotion].yStart));
}
else
{
setLowerBody(stdMotions[currMotion].yStart);
}
}
protected void Update()
{
float timestamp = Time.time;
Data.X_POS xPos = new Data.X_POS(head, leftHand, rightHand);
Data.Y_POS yPos = new Data.Y_POS(leftFoot, rightFoot, leftKnee, rightKnee, waist);
bool continuous = movingDetect.isMoving();
record.recordData(timestamp, xPos, yPos, continuous);
movingDetect.update(record);
updateCaliSkeleton();
}
protected void setLowerBody(Data.Y_POS yPos)
{
List <GameObject> objs = new List <GameObject>();
objs.Add(leftFoot);
objs.Add(rightFoot);
objs.Add(leftKnee);
objs.Add(rightKnee);
objs.Add(waist);
int cnt = 0;
for (int i = 0; i < yPos.N; i += 7)
{
objs[cnt].transform.position = objs[cnt].transform.position * smoothK + new Vector3(yPos.vec[i + 0], yPos.vec[i + 1], yPos.vec[i + 2]) * (1 - smoothK);
objs[cnt].transform.rotation = new Quaternion(yPos.vec[i + 3], yPos.vec[i + 4], yPos.vec[i + 5], yPos.vec[i + 6]);
cnt++;
}
}
private Data.POS getPOS(bool isXPos)
{
Data.POS pos;
int s = 0, t = 3;
if (isXPos)
{
pos = new Data.X_POS();
}
else
{
pos = new Data.Y_POS();
s = 3;
t = 8;
}
for (int i = s; i < t; i++)
{
Vector3 position = new Vector3();
Quaternion rotation = new Quaternion();
if (objects[i] != null)
{
position = objects[i].transform.position;
rotation = objects[i].transform.rotation;
}
pos.vec[(i - s) * 7 + 0] = position.x;
pos.vec[(i - s) * 7 + 1] = position.y;
pos.vec[(i - s) * 7 + 2] = position.z;
pos.vec[(i - s) * 7 + 3] = rotation.x;
pos.vec[(i - s) * 7 + 4] = rotation.y;
pos.vec[(i - s) * 7 + 5] = rotation.z;
pos.vec[(i - s) * 7 + 6] = rotation.w;
}
return(pos);
}
