public static BVH_IO Record_Start(Body body, KinectSkeleton skel, string fileName, string path)
{
try
{
int initFrames = 10;
BVH_IO bfhFile = new BVH_IO(fileName, path);
if (bfhFile != null)
{
if (bfhFile.isRecording == true && bfhFile.isInitializing == true)
{
bfhFile.Entry(skel);
//if (bfhFile.intializingCounter > initFrames)
//{
bfhFile.startWritingEntry();
// }
}
if (bfhFile.isRecording == true && bfhFile.isInitializing == false)
{
bfhFile.WriteMotions(skel, body);
//this.textBox_sensorStatus.Text = "Record";
//this.textBox_sensorStatus.BackColor = Color.Green;
}
}
return(bfhFile);
}
catch (Exception err)
{
System.Windows.Forms.MessageBox.Show("Error in BVH_IO.Record_Start: " + err.Message);
return(null);
}
}
public static float[] getBoneVectorOutofJointPosition(BVHBone bvhBone, KinectSkeleton skel)
{
float[] boneVector = new float[3] {
0, 0, 0
};
float[] boneVectorParent = new float[3] {
0, 0, 0
};
string boneName = bvhBone.Name;
JointType Joint;
if (bvhBone.Root == true)
{
boneVector = new float[3] {
0, 0, 0
};
}
else
{
if (bvhBone.IsKinectJoint == true)
{
Joint = KinectSkeletonBVH.String2JointType(boneName);
boneVector[0] = skel.Joints[Joint].X;
boneVector[1] = skel.Joints[Joint].Y;
boneVector[2] = skel.Joints[Joint].Z;
try
{
Joint = KinectSkeletonBVH.String2JointType(bvhBone.Parent.Name);
}
catch
{
Joint = KinectSkeletonBVH.String2JointType(bvhBone.Parent.Parent.Name);
}
boneVector[0] -= skel.Joints[Joint].X;
boneVector[1] -= skel.Joints[Joint].Y;
boneVector[2] -= skel.Joints[Joint].Z;
}
}
return(boneVector);
}
//eigentliche Schreibarbeit:
public void Entry(KinectSkeleton skel)
{
this.intializingCounter++;
for (int k = 0; k < bvhSkeleton.Bones.Count; k++)
{
float[] bonevector = KinectSkeletonBVH.getBoneVectorOutofJointPosition(bvhSkeleton.Bones[k], skel);
{
if (this.intializingCounter == 1)
{
tempOffsetMatrix[0, k] = (float)Math.Round(bonevector[0] * 100, 2);
tempOffsetMatrix[1, k] = (float)Math.Round(bonevector[1] * 100, 2);
tempOffsetMatrix[2, k] = (float)Math.Round(bonevector[2] * 100, 2);
}
else
{
tempOffsetMatrix[0, k] = (float)(this.intializingCounter * tempOffsetMatrix[0, k] + Math.Round(bonevector[0] * 100, 2)) / (this.intializingCounter + 1);
tempOffsetMatrix[1, k] = (float)(this.intializingCounter * tempOffsetMatrix[1, k] + Math.Round(bonevector[1] * 100, 2)) / (this.intializingCounter + 1);
tempOffsetMatrix[2, k] = (float)(this.intializingCounter * tempOffsetMatrix[1, k] + Math.Round(bonevector[2] * 100, 2)) / (this.intializingCounter + 1);
}
}
}
}
public void WriteMotions(KinectSkeleton skel, Body body)
{
sw.Start(); //Aufnahme der Bewegungen beginnt
for (int k = 0; k < bvhSkeletonWritten.Bones.Count; k++)
{
if (bvhSkeletonWritten.Bones[k].End == false)
{
float[] degVec = new float[3];
degVec = KinectSkeletonBVH.getEulerFromBone(bvhSkeletonWritten.Bones[k], skel, body);
int indexOffset = 0;
if (bvhSkeletonWritten.Bones[k].Root == true)
{
indexOffset = 3;
}
tempMotionVektor[bvhSkeletonWritten.Bones[k].MotionSpace + indexOffset] = degVec[0];
tempMotionVektor[bvhSkeletonWritten.Bones[k].MotionSpace + 1 + indexOffset] = degVec[1];
tempMotionVektor[bvhSkeletonWritten.Bones[k].MotionSpace + 2 + indexOffset] = degVec[2];
//// Textbox setzen
//string boneName = bvhSkeletonWritten.Bones[k].Name;
//if (boneName == textFeld.getDropDownJoint)
//{
// //Rotation
// string textBox = Math.Round(degVec[0], 1).ToString() + " " + Math.Round(degVec[1], 1).ToString() + " " + Math.Round(degVec[2], 1).ToString();
// textFeld.setTextBoxAngles = textBox;
// //Position
// JointType KinectJoint = KinectSkeletonBVH.getJointTypeFromBVHBone(bvhSkeletonWritten.Bones[k]);
// float x = skel.Joints[KinectJoint].Position.X;
// float y = skel.Joints[KinectJoint].Position.Y;
// float z = skel.Joints[KinectJoint].Position.Z;
// textFeld.setTextPosition = Math.Round(x, 2).ToString() + " " + Math.Round(y, 2).ToString() + " " + Math.Round(z, 2).ToString();
// //Length
// BVHBone tempBone = bvhSkeletonWritten.Bones.Find(i => i.Name == KinectJoint.ToString());
// float[] boneVec = KinectSkeletonBVH.getBoneVectorOutofJointPosition(tempBone, skel);
// float length = Math.Sqrt(Math.Pow(boneVec[0], 2) + Math.Pow(boneVec[1], 2) + Math.Pow(boneVec[2], 2));
// length = Math.Round(length, 2);
// textFeld.setTextBoxLength = length.ToString();
//}
}
}
//Root Bewegung
tempMotionVektor[0] = (float)-Math.Round(skel.Joints[JointType.SpineMid].X * 100, 2);
tempMotionVektor[1] = (float)Math.Round(skel.Joints[JointType.SpineMid].Y * 100, 2) + 120;
tempMotionVektor[2] = 300 - (float)Math.Round(skel.Joints[JointType.SpineMid].Z * 100, 2);
for (int i = 0; i < 3; i++)
{
if (tempMotionVektor[i] == float.NaN)
{
tempMotionVektor[i] = 0;
}
}
writeMotion(tempMotionVektor);
file.Flush();
elapsedTimeSec = (float)Math.Round(Convert.ToDouble(sw.ElapsedMilliseconds) / 1000, 2);
//textFeld.setTextBoxElapsedTime = elapsedTimeSec.ToString();
//textFeld.setTextBoxCapturedFrames = frameCounter.ToString();
avgFrameRate = (float)Math.Round(frameCounter / elapsedTimeSec, 2);
// textFeld.setTextBoxFrameRate = avgFrameRate.ToString();
}
public static float[] getEulerFromBone(BVHBone bone, KinectSkeleton skel, Body body)
{
float[] degVec = new float[3] {
0, 0, 0
};
float[] correctionDegVec = new float[3] {
0, 0, 0
};
JointType kinectJoint = new JointType();
JointType ParentKinectJoint = new JointType();
bool noData = false;
kinectJoint = getJointTypeFromBVHBone(bone);
switch (bone.Name)
{
case "SpineBase2":
//correctionDegVec[0] = 150;
correctionDegVec[0] = -30;
//correctionDegVec[0] = -30;
break;
case "ShoulderLeft":
correctionDegVec[0] = 30;
break;
case "ShoulderRight":
correctionDegVec[0] = 30;
break;
case "HipRight":
correctionDegVec[0] = -10;
break;
case "HipLeft":
correctionDegVec[0] = -10;
break;
case "KneeLeft":
correctionDegVec[0] = 10;
break;
case "KneeRight":
correctionDegVec[0] = 10;
break;
case "SpineShoulder":
//correctionDegVec[0] = -20;
break;
case "Neck":
correctionDegVec[0] = -20;
break;
case "CollarRight":
noData = true;
break;
case "CollarLeft":
noData = true;
break;
case "Spine":
//Gibt die Rotation der Wirbelsäule zwischen Spine Joint und Shoulder Center an.
degVec[0] = 30;
degVec[1] = 0;
degVec[2] = 0;
noData = true;
break;
case "Head": //Informationen sind in "Neck"
noData = true;
break;
case "AnkleRight":
noData = true;
break;
case "AnkleLeft":
noData = true;
break;
default:
break;
}
if (bone.Root == false)
{
//Das BVH Skelett hat mehr Knochen wie das Kinect Skelett, diese Joints haben dauerthaft die Rotationen 0 0 0
if (noData == false)
{
Quaternion tempQuat;
if (!(bone.Name == "HipRight" || bone.Name == "HipLeft" || bone.Name == "ShoulderLeft" || bone.Name == "ShoulderRight" || bone.Name == "SpineBase2"))
{
tempQuat = MathHelper.Vector4ToQuat(body.JointOrientations[kinectJoint].Orientation);
tempQuat = MathHelper.Vector4ToQuat(body.JointOrientations[kinectJoint].Orientation);
degVec = MathHelper.quat2Deg(tempQuat);
if (bone.Name == "SpineBase2")
{
degVec[1] = 0;
degVec[2] = -degVec[2];
}
//Beine
if (bone.Axis == TransAxis.nY)
{
degVec[0] = -degVec[0];
degVec[1] = -degVec[1];
degVec[2] = degVec[2];
}
//Rechter Arm
if (bone.Axis == TransAxis.nX && bone.Name != "ShoulderRight")
{
float[] tempDecVec = new float[3] {
degVec[0], degVec[1], degVec[2]
};
degVec[0] = -tempDecVec[2];
degVec[1] = -tempDecVec[1];
degVec[2] = -tempDecVec[0];
}
/*
* //Rechte Schulter
* if (bone.Name == "ShoulderRight")
* {
* float[] tempDecVec = new float[3] { degVec[0], degVec[1], degVec[2] };
* degVec[0] = -tempDecVec[0];
* degVec[1] = tempDecVec[1];
* degVec[2] = tempDecVec[2];
*
* }
*
* //Linke Schulter
* if (bone.Name == "ShoulderLeft")
* {
* float[] tempDecVec = new float[3] { degVec[0], degVec[1], degVec[2] };
* degVec[0] = -tempDecVec[0];
* degVec[1] = tempDecVec[1];
* degVec[2] = tempDecVec[2];
*
* }
*/
//Linker Arm
if (bone.Axis == TransAxis.X && bone.Name != "ShoulderLeft")
{
float[] tempDecVec = new float[3] {
degVec[0], degVec[1], degVec[2]
};
degVec[0] = tempDecVec[2];
degVec[1] = tempDecVec[1];
degVec[2] = tempDecVec[0];
}
}
else
{
//Rotation per "Hand" ausrechnen mithilfe von Vektoren. Ist nötig, da dass BVH Skelett an den Hüft- und Schulterknochen nicht mit dem Kinect Skelett übereinstimmen
Vector3 vec = new Vector3();
Vector3 axis = new Vector3();
switch (bone.Name)
{
case "HipRight":
axis = new Vector3(0, -1, 0);
ParentKinectJoint = JointType.HipRight;
break;
case "HipLeft":
axis = new Vector3(0, -1, 0);
ParentKinectJoint = JointType.HipLeft;
break;
case "SpineBase2":
axis = new Vector3(0, 1, 0);
ParentKinectJoint = JointType.SpineBase;
kinectJoint = JointType.SpineShoulder;
break;
case "ShoulderRight":
axis = new Vector3(1, 0, 0);
ParentKinectJoint = JointType.ShoulderRight;
break;
case "ShoulderLeft":
axis = new Vector3(-1, 0, 0);
ParentKinectJoint = JointType.ShoulderLeft;
break;
}
float skal = (skel.Joints[kinectJoint].Z / skel.Joints[ParentKinectJoint].Z);
skal = 1;
vec.X = skel.Joints[kinectJoint].X * skal - skel.Joints[ParentKinectJoint].X * 1 / skal;
vec.Y = skel.Joints[kinectJoint].Y * skal - skel.Joints[ParentKinectJoint].Y * 1 / skal;
vec.Z = skel.Joints[kinectJoint].Z - skel.Joints[ParentKinectJoint].Z;
vec.Normalize();
if (bone.Name == "ShoulderLeft" || bone.Name == "ShoulderRight")
{
float[] rotationOffset = new float[3];
//rotationOffset = MathHelper.quat2Deg(skel.BoneOrientations[JointType.SpineShoulder].AbsoluteRotation.Quaternion);
rotationOffset = MathHelper.VectorToDeg(body.JointOrientations[JointType.SpineShoulder].Orientation);
Matrix3 rotMat = Matrix3Extension.GetRotationMatrix(-(rotationOffset[0] * (float)Math.PI / 180) - 180, 0, 0);
Vector3 vec2 = vec.MultiplyByMatrix(rotMat);
tempQuat = MathHelper.getQuaternion(axis, vec2);
degVec = MathHelper.quat2Deg(tempQuat);
degVec[0] = degVec[0];
degVec[1] = degVec[1]; //+ rotationOffset[1];
degVec[2] = degVec[2];
degVec[2] = -degVec[2];
}
if (bone.Name == "SpineBase2")
{
float[] rotationOffset = new float[3] {
0, 0, 0
};
rotationOffset = MathHelper.VectorToDeg(body.JointOrientations[JointType.SpineBase].Orientation);
Vector3 vec2 = vec.MultiplyByMatrix(Matrix3Extension.GetRotationMatrixY(-rotationOffset[1] * (float)Math.PI / 180));
tempQuat = MathHelper.getQuaternion(axis, vec2);
degVec = MathHelper.quat2Deg(tempQuat);
degVec[1] = 0;
degVec[0] = -degVec[0];
degVec[2] = -degVec[2];
}
if (bone.Name == "HipRight" || bone.Name == "HipLeft")
{
float[] rotationOffset = new float[3] {
0, 0, 0
};
rotationOffset = MathHelper.VectorToDeg(body.JointOrientations[JointType.SpineBase].Orientation);
Vector3 vec2 = vec.MultiplyByMatrix(Matrix3Extension.GetRotationMatrixY(-rotationOffset[1] * (float)Math.PI / 180));
tempQuat = MathHelper.getQuaternion(axis, vec2);
degVec = MathHelper.quat2Deg(tempQuat);
degVec[0] = -degVec[0];
degVec[1] = -degVec[1]; //Nur Yaw WInkel Wichtig
degVec[2] = -degVec[2];
}
}
}
}
else
{
//Kinect Kamera Koordinatensystem ist genau spiegelverkehrt zum User Koordinatensystem
Microsoft.Kinect.Vector4 tempQuat = body.JointOrientations[kinectJoint].Orientation;
degVec = MathHelper.VectorToDeg(tempQuat);
//Hüfte bleibt immer parallel zum Boden ausgerichtet. Nur Oberkörper kann sich "verbiegen"
degVec[0] = 0;
degVec[1] = -degVec[1]; //Drehung um die eigene Achse "YAW Winkel"
degVec[2] = 0;
}
//Korrektur
degVec = MathHelper.addArray(degVec, correctionDegVec);
//Falls WInkel über 180 Grad ist
for (int k = 0; k < 3; k++)
{
if (degVec[k] > 180)
{
degVec[k] -= 360;
}
else if (degVec[k] < -180)
{
degVec[k] += 360;
}
}
bone.setRotOffset(degVec[0], degVec[1], degVec[2]); //wird eigentlich nicht benötigt
return(degVec);
}