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); }