//bodyDataの入れ替え
private void updateCheckData(int index)
{
Console.WriteLine("updateCheckData");
CheckData checkData = checkDatas[index];
int numberForArraySort = NUMBER_OF_FRAME - 1;
//time
long dt = GetUnixTime(DateTime.Now);
//このindexのnowでーた取得
Body body = bodies[index];
double nowHeadX = Math.Round(body.Joints[JointType.Head].Position.X * 30, 0);
double nowHeadY = Math.Round(body.Joints[JointType.Head].Position.Y * 30, 0);
double nowHeadZ = Math.Round(body.Joints[JointType.Head].Position.Z * 30, 0);
double nowMiddleX = Math.Round(body.Joints[JointType.SpineMid].Position.X * 30, 0);
double nowMiddleY = Math.Round(body.Joints[JointType.SpineMid].Position.Y * 30, 0);
double nowMiddleZ = Math.Round(body.Joints[JointType.SpineMid].Position.Z * 30, 0);
//初期の時
if (checkData == null)
{
checkData = new CheckData();
if (index == 0)
{
sw.Start();
Console.WriteLine("***?????? ストップウォッチ開始");
}
}
//data代入
double[] headXInfo = checkData.headXInfo;
double[] headYInfo = checkData.headYInfo;
double[] headZInfo = checkData.headZInfo;
double[] middleXInfo = checkData.middleXInfo;
double[] middleYInfo = checkData.middleYInfo;
double[] middleZInfo = checkData.middleZInfo;
long[] time = checkData.time;
int length = headZInfo.Length;
Console.WriteLine("***length:{0}", length);
if (length < NUMBER_OF_FRAME)//配列埋まりきっていない時の処理-配列の長さを図ってその次のところに追加
{
Array.Resize(ref headXInfo, length + 1);
Array.Resize(ref headYInfo, length + 1);
Array.Resize(ref headZInfo, length + 1);
Array.Resize(ref middleXInfo, length + 1);
Array.Resize(ref middleYInfo, length + 1);
Array.Resize(ref middleZInfo, length + 1);
Array.Resize(ref time, length + 1);
//nowDataを末尾に追加
headXInfo[length] = nowHeadX;
headYInfo[length] = nowHeadY;
headZInfo[length] = nowHeadZ;
middleXInfo[length] = nowMiddleX;
middleYInfo[length] = nowMiddleY;
middleZInfo[length] = nowMiddleZ;
time[length] = dt;
}
else//配列が埋まりきっているとき-配列の中身入れ替え
{
//Info中身入れ替え
for (int i = 0; i < numberForArraySort; ++i)
{
headXInfo[i] = headXInfo[i + 1];
headYInfo[i] = headYInfo[i + 1];
headZInfo[i] = headZInfo[i + 1];
middleXInfo[i] = middleXInfo[i + 1];
middleYInfo[i] = middleYInfo[i + 1];
middleZInfo[i] = middleZInfo[i + 1];
time[i] = time[i + 1];
}
headXInfo[numberForArraySort] = nowHeadX;//nowDataをいれる
headYInfo[numberForArraySort] = nowHeadY;
headZInfo[numberForArraySort] = nowHeadZ;
middleXInfo[numberForArraySort] = nowMiddleX;//nowDataをいれる
middleYInfo[numberForArraySort] = nowMiddleY;
middleZInfo[numberForArraySort] = nowMiddleZ;
time[numberForArraySort] = dt;
if (index == 0)
{
sw.Stop();
Console.WriteLine("***?????? 時間:{0}", sw.Elapsed);
}
}
foreach (int i in headZInfo)
{
// Console.WriteLine(i);
}
//データ保存
checkData.middleXInfo = middleXInfo;
checkData.middleYInfo = middleYInfo;
checkData.middleZInfo = middleZInfo;
checkData.headXInfo = headXInfo;
checkData.headYInfo = headYInfo;
checkData.headZInfo = headZInfo;
checkData.time = time;
checkDatas[index] = checkData;
}
private void check_BodySpead(int index)
{
Console.WriteLine("CheckHeadSpead");
CheckData checkData = checkDatas[index];
double[] headXInfo = checkData.headXInfo;
double[] headYInfo = checkData.headYInfo;
double[] headZInfo = checkData.headZInfo;
double[] middleXInfo = checkData.middleXInfo;
double[] middleYInfo = checkData.middleYInfo;
double[] middleZInfo = checkData.middleZInfo;
long[] time = checkData.time;
Body body = bodies[index];
if (!body.IsTracked)
{
return;
}
double headX = Math.Round(body.Joints[JointType.Head].Position.X * 30, 0);
double headY = Math.Round(body.Joints[JointType.Head].Position.Y * 30, 0);
double headZ = Math.Round(body.Joints[JointType.Head].Position.Z * 30, 0);
double middleX = Math.Round(body.Joints[JointType.SpineMid].Position.X * 30, 0);
double middleY = Math.Round(body.Joints[JointType.SpineMid].Position.Y * 30, 0);
double middleZ = Math.Round(body.Joints[JointType.SpineMid].Position.Z * 30, 0);
double oldHeadX = headXInfo.First();
double oldHeadY = headYInfo.First();
double oldHeadZ = headZInfo.First();
double oldMiddleX = middleXInfo.First();
double oldMiddleY = middleYInfo.First();
double oldMiddleZ = middleZInfo.First();
double deltaHeadX = System.Math.Pow((headX - oldHeadX), 2);
double deltaHeadY = System.Math.Pow((headY - oldHeadY), 2);
double deltaHeadZ = System.Math.Pow((headZ - oldHeadZ), 2);
double deltaMiddleX = System.Math.Pow((middleX - oldMiddleX), 2);
double deltaMiddleY = System.Math.Pow((middleY - oldMiddleY), 2);
double deltaMiddleZ = System.Math.Pow((middleZ - oldMiddleZ), 2);
double headMove3D = System.Math.Sqrt(deltaHeadX + deltaHeadY + deltaHeadZ);
double middleMove3D = System.Math.Sqrt(deltaMiddleX + deltaMiddleY + deltaMiddleZ);
double difTime = GetUnixTime(DateTime.Now) - time.First();
double speedHead = headMove3D / difTime;
double speedMiddle = middleMove3D / difTime;
Console.WriteLine("now headX:{0}", headX);
Console.WriteLine("old headX:{0}", oldHeadX);
Console.WriteLine("now headY:{0}", headY);
Console.WriteLine("old headY:{0}", oldHeadY);
Console.WriteLine("now headZ:{0}", headZ);
Console.WriteLine("old headZ:{0}", oldHeadZ);
Console.WriteLine("******Move Head 3D : {0}******", headMove3D);
Console.WriteLine("******Move Middle 3D : {0}******", middleMove3D);
Console.WriteLine("******Dif Time : {0}******", difTime);
Console.WriteLine("******Speed Head : {0}******", speedHead);
Console.WriteLine("******Speed Middle : {0}******", speedMiddle);
double[] writeData = { headMove3D, middleMove3D, difTime, speedHead, speedMiddle };
if (headZ < 120 && headZ > 10)//ボーントラッキングの範囲外に出たときにheadｚがおかしくなって誤検知が起きるのでここで調整
{
if (headMove3D > 38 && middleMove3D > 36)
{
Console.WriteLine("************Move Fall Down**************");
}
if (speedHead > 0.016 && speedMiddle > 0.015)
{
Console.WriteLine("************Speed Fall Down**************");
this.fallDown = true;
NotificationBlock.Background = Brushes.Red;
}
}
//writeDataToExcel(writeData);
//this.Close();
}