public MyBone(MyBone Parent, string Name, int nrChannels, TransAxis Axis, bool IsKinectJoint)
{
parent = Parent;
index += index;
name = Name;
isKinectJoint = IsKinectJoint;
axis = Axis;
if (parent != null)
depth = parent.Depth + 1;
else
{
depth = 0;
root = true;
}
channels = new PositionRotation[nrChannels];
int ind = 5;
for (int k = nrChannels-1; k >= 0; k--)
{
channels[k] = (PositionRotation)ind;
ind--;
}
}
public static double[] getBoneVectorOutofJointPosition(MyBone NGEBone, Skeleton skel)
{
double[] boneVector = new double[3] { 0, 0, 0 };
double[] boneVectorParent = new double[3] { 0, 0, 0 };
string boneName = NGEBone.Name;
JointType Joint;
if (NGEBone.Root == true)
{
boneVector = new double[3] { 0, 0, 0 };
}
else
{
if (NGEBone.IsKinectJoint == true)
{
Joint = MyKinectSkeleton.String2JointType(boneName);
boneVector[0] = skel.Joints[Joint].Position.X;
boneVector[1] = skel.Joints[Joint].Position.Y;
boneVector[2] = skel.Joints[Joint].Position.Z;
try
{
Joint = MyKinectSkeleton.String2JointType(NGEBone.Parent.Name);
}
catch
{
Joint = MyKinectSkeleton.String2JointType(NGEBone.Parent.Parent.Name);
}
boneVector[0] -= skel.Joints[Joint].Position.X;
boneVector[1] -= skel.Joints[Joint].Position.Y;
boneVector[2] -= skel.Joints[Joint].Position.Z;
}
}
return boneVector;
}
public static JointType getJointTypeFromMyBone(MyBone bone)
{
JointType kinectJoint = new JointType();
switch (bone.Name)
{
case "HipCenter":
kinectJoint = JointType.HipCenter;
break;
case "HipCenter2":
kinectJoint = JointType.HipCenter;
break;
case "Spine":
kinectJoint = JointType.Spine;
break;
case "ShoulderCenter":
kinectJoint = JointType.ShoulderCenter;
break;
case "Neck":
kinectJoint = JointType.Head;
break;
case "Head":
kinectJoint = JointType.Head;
break;
case "CollarRight":
kinectJoint = JointType.ShoulderRight;
break;
case "ShoulderRight":
kinectJoint = JointType.ElbowRight;
break;
case "ElbowRight":
kinectJoint = JointType.WristRight;
break;
case "WristRight":
kinectJoint = JointType.HandRight;
break;
case "CollarLeft":
kinectJoint = JointType.ShoulderLeft;
break;
case "ShoulderLeft":
kinectJoint = JointType.ElbowLeft;
break;
case "ElbowLeft":
kinectJoint = JointType.WristLeft;
break;
case "WristLeft":
kinectJoint = JointType.HandLeft;
break;
case "HipLeft":
kinectJoint = JointType.KneeLeft;
break;
case "KneeLeft":
kinectJoint = JointType.AnkleLeft;
break;
case "AnkleLeft":
kinectJoint = JointType.FootLeft;
break;
case "HipRight":
kinectJoint = JointType.KneeRight;
break;
case "KneeRight":
kinectJoint = JointType.AnkleRight;
break;
case "AnkleRight":
kinectJoint = JointType.FootRight;
break;
}
return kinectJoint;
}
public static void AddKinectSkeleton(MySkeleton Skeleton)
{
//Die Person steht falsch herum im Koordinatensystem der Kinect! Es wird erst beim Abspeichern korrigiert, weshalb die Verarbeitung noch mit umgekehrten Koordinaten erfolgt
MyBone hipCenter = new MyBone(null, JointType.HipCenter.ToString(), 6, TransAxis.None, true);
MyBone hipCenter2 = new MyBone(hipCenter, "HipCenter2", 3, TransAxis.Y, false);
MyBone spine = new MyBone(hipCenter2, JointType.Spine.ToString(), 3, TransAxis.Y, true);
MyBone shoulderCenter = new MyBone(spine, JointType.ShoulderCenter.ToString(), 3, TransAxis.Y, true);
MyBone collarLeft = new MyBone(shoulderCenter, "CollarLeft", 3, TransAxis.X, false);
MyBone shoulderLeft = new MyBone(collarLeft, JointType.ShoulderLeft.ToString(), 3, TransAxis.X, true);
MyBone elbowLeft = new MyBone(shoulderLeft, JointType.ElbowLeft.ToString(), 3, TransAxis.X, true);
MyBone wristLeft = new MyBone(elbowLeft, JointType.WristLeft.ToString(), 3, TransAxis.X, true);
MyBone handLeft = new MyBone(wristLeft, JointType.HandLeft.ToString(), 0, TransAxis.X, true);
MyBone neck = new MyBone(shoulderCenter, "Neck", 3, TransAxis.Y, false);
MyBone head = new MyBone(neck, JointType.Head.ToString(), 3, TransAxis.Y, true);
MyBone headtop = new MyBone(head, "Headtop", 0, TransAxis.None, false);
MyBone collarRight = new MyBone(shoulderCenter, "CollarRight", 3, TransAxis.nX, false);
MyBone shoulderRight = new MyBone(collarRight, JointType.ShoulderRight.ToString(), 3, TransAxis.nX, true);
MyBone elbowRight = new MyBone(shoulderRight, JointType.ElbowRight.ToString(), 3, TransAxis.nX, true);
MyBone wristRight = new MyBone(elbowRight, JointType.WristRight.ToString(), 3, TransAxis.nX, true);
MyBone handRight = new MyBone(wristRight, JointType.HandRight.ToString(), 0, TransAxis.nX, true);
MyBone hipLeft = new MyBone(hipCenter, JointType.HipLeft.ToString(), 3, TransAxis.X, true);
MyBone kneeLeft = new MyBone(hipLeft, JointType.KneeLeft.ToString(), 3, TransAxis.nY, true);
MyBone ankleLeft = new MyBone(kneeLeft, JointType.AnkleLeft.ToString(), 3, TransAxis.nY, true);
MyBone footLeft = new MyBone(ankleLeft, JointType.FootLeft.ToString(), 0, TransAxis.Z, true);
MyBone hipRight = new MyBone(hipCenter, JointType.HipRight.ToString(), 3, TransAxis.nX, true);
MyBone kneeRight = new MyBone(hipRight, JointType.KneeRight.ToString(), 3, TransAxis.nY, true);
MyBone ankleRight = new MyBone(kneeRight, JointType.AnkleRight.ToString(), 3, TransAxis.nY, true);
MyBone footRight = new MyBone(ankleRight, JointType.FootRight.ToString(), 0, TransAxis.Z, true);
Skeleton.AddBone(hipCenter);
Skeleton.AddBone(hipCenter2);
Skeleton.AddBone(spine);
Skeleton.AddBone(shoulderCenter);
Skeleton.AddBone(collarLeft);
Skeleton.AddBone(shoulderLeft);
Skeleton.AddBone(elbowLeft);
Skeleton.AddBone(wristLeft);
Skeleton.AddBone(handLeft);
Skeleton.AddBone(neck);
Skeleton.AddBone(head);
Skeleton.AddBone(headtop);
Skeleton.AddBone(collarRight);
Skeleton.AddBone(shoulderRight);
Skeleton.AddBone(elbowRight);
Skeleton.AddBone(wristRight);
Skeleton.AddBone(handRight);
Skeleton.AddBone(hipLeft);
Skeleton.AddBone(kneeLeft);
Skeleton.AddBone(ankleLeft);
Skeleton.AddBone(footLeft);
Skeleton.AddBone(hipRight);
Skeleton.AddBone(kneeRight);
Skeleton.AddBone(ankleRight);
Skeleton.AddBone(footRight);
Skeleton.FinalizeMySkeleton();
}
public static double[] getEulerFromBone(MyBone bone, Skeleton skel)
{
double[] degVec = new double[3] { 0, 0, 0 };
double[] correctionDegVec = new double[3] { 0, 0, 0 };
JointType kinectJoint = new JointType();
JointType ParentKinectJoint = new JointType();
bool noData = false;
kinectJoint = getJointTypeFromMyBone(bone);
switch (bone.Name)
{
case "HipCenter2":
//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 "ShoulderCenter":
//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 NGE 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 == "HipCenter2"))
{
tempQuat = MathHelper.vector42Quat(skel.BoneOrientations[kinectJoint].HierarchicalRotation.Quaternion);
degVec = MathHelper.quat2Deg(tempQuat);
if (bone.Name == "HipCenter2")
{
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")
{
double[] tempDecVec = new double[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")
{
double[] tempDecVec = new double[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")
{
double[] tempDecVec = new double[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")
{
double[] tempDecVec = new double[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 NGE Skelett an den Hüft- und Schulterknochen nicht mit dem Kinect Skelett übereinstimmen
Vector3D vec = new Vector3D();
Vector3D axis = new Vector3D();
switch (bone.Name)
{
case "HipRight":
axis = new Vector3D(0, -1, 0);
ParentKinectJoint = JointType.HipRight;
break;
case "HipLeft":
axis = new Vector3D(0, -1, 0);
ParentKinectJoint = JointType.HipLeft;
break;
case "HipCenter2":
axis = new Vector3D(0, 1, 0);
ParentKinectJoint = JointType.Spine;
kinectJoint = JointType.ShoulderCenter;
break;
case "ShoulderRight":
axis = new Vector3D(1, 0, 0);
ParentKinectJoint = JointType.ShoulderRight;
break;
case "ShoulderLeft":
axis = new Vector3D(-1, 0, 0);
ParentKinectJoint = JointType.ShoulderLeft;
break;
}
double skal = (skel.Joints[kinectJoint].Position.Z / skel.Joints[ParentKinectJoint].Position.Z);
skal = 1;
vec.X = skel.Joints[kinectJoint].Position.X * skal - skel.Joints[ParentKinectJoint].Position.X * 1/skal;
vec.Y = skel.Joints[kinectJoint].Position.Y * skal - skel.Joints[ParentKinectJoint].Position.Y * 1 / skal;
vec.Z = skel.Joints[kinectJoint].Position.Z - skel.Joints[ParentKinectJoint].Position.Z;
vec.Normalize();
if (bone.Name == "ShoulderLeft" || bone.Name == "ShoulderRight")
{
double[] rotationOffset = new double[3];
rotationOffset = MathHelper.quat2Deg(skel.BoneOrientations[JointType.ShoulderCenter].AbsoluteRotation.Quaternion);
Matrix3D rotMat = MathHelper.GetRotationMatrix( -(rotationOffset[0] * Math.PI / 180) -180, 0, 0);
Vector3D vec2 = Vector3D.Multiply(vec, 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 == "HipCenter2")
{
double[] rotationOffset = new double[3] { 0, 0, 0 };
rotationOffset = MathHelper.quat2Deg(skel.BoneOrientations[JointType.HipCenter].AbsoluteRotation.Quaternion);
Vector3D vec2 = Vector3D.Multiply(vec, MathHelper.GetRotationMatrixY(-rotationOffset[1] * 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")
{
double[] rotationOffset = new double[3]{0,0,0};
rotationOffset = MathHelper.quat2Deg(skel.BoneOrientations[JointType.HipCenter].AbsoluteRotation.Quaternion);
Vector3D vec2 = Vector3D.Multiply(vec, MathHelper.GetRotationMatrixY(-rotationOffset[1] * 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
Vector4 tempQuat = skel.BoneOrientations[kinectJoint].AbsoluteRotation.Quaternion;
degVec = MathHelper.quat2Deg(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;
}
public void AddBone(MyBone Bone)
{
if (!Bones.Contains(Bone))
{
bones.Add(Bone);
}
}
private string writeChannels(MyBone bone)
{
string output = "CHANNELS " + bone.Channels.Length.ToString() + " ";
for (int k = 0; k < bone.Channels.Length; k++)
{
output += bone.Channels[k].ToString() + " ";
}
return output;
}
private string calcTabs(MyBone currentBone)
{
int depth = currentBone.Depth;
string tabs = "";
for (int k = 0; k < currentBone.Depth; k++)
{
tabs += "\t";
}
return tabs;
}
