void checkMoving()
{
Data.X_POS xPos = getXPos();
float speed = Data.POS.dist(xPos, lastXPos) / Time.deltaTime;
if (speed >= speedThreshold)
{
movingTime += Time.deltaTime;
stopTime = 0f;
if (movingTime >= beginThreshold)
{
moveScreen.text = "Moving";
}
}
else
{
movingTime = 0f;
stopTime += Time.deltaTime;
if (stopTime >= endThreshold)
{
moveScreen.text = "Stop";
}
}
lastXPos = xPos;
}
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 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();
}
void checkHoming()
{
Data.X_POS xPos = getXPos();
float dist = Data.POS.dist(xPos, homePos);
if (dist > homeThreshold)
{
homeScreen.text = "Away";
}
else
{
homeScreen.text = "Home";
}
}
void Update()
{
if (t == -1)
{
return;
}
if (t < motion.timestamp.Count)
{
Data.X_POS xPos = new Data.X_POS(motion.xPos[t] + xStart);
leftHand.transform.position = new Vector3(xPos.vec[7], xPos.vec[8], xPos.vec[9]);
leftHand.transform.rotation = new Quaternion(xPos.vec[10], xPos.vec[11], xPos.vec[12], xPos.vec[13]);
rightHand.transform.position = new Vector3(xPos.vec[14], xPos.vec[15], xPos.vec[16]);
rightHand.transform.rotation = new Quaternion(xPos.vec[17], xPos.vec[18], xPos.vec[19], xPos.vec[20]);
}
t++;
if (t >= motion.timestamp.Count)
{
t = -1;
}
}
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);
}
public float getHandSpeed()
{
int frames = record.getIndex() - movingDetect.getStartIndex();
if (frames > 100)
{
frames = 100;
}
List <float> speedList = new List <float>();
for (int i = frames; i >= 0; i--)
{
Data.X_POS xPos = record.getXSpeedSmooth(i);
float speed = 0f;
speed += Mathf.Sqrt(Mathf.Pow(xPos.vec[7], 2f) + Mathf.Pow(xPos.vec[8], 2f) + Mathf.Pow(xPos.vec[9], 2f));
speed += Mathf.Sqrt(Mathf.Pow(xPos.vec[14], 2f) + Mathf.Pow(xPos.vec[15], 2f) + Mathf.Pow(xPos.vec[16], 2f));
speed /= 2f;
speedList.Add(speed);
}
speedList.Sort();
float ret = speedList[(int)(speedList.Count * 0.9f) - 1];
return(ret);
}
public void playMotion(Data.X_POS xStart)
{
this.xStart = xStart;
t = 0;
}
