public static float getElbowRoll_Left(Skeleton skeleton)
{
// Get fitting joints
Joint shoulder_j = skeleton.Joints[JointType.ShoulderLeft];
Joint elbow_j = skeleton.Joints[JointType.ElbowLeft];
Joint hand_j = skeleton.Joints[JointType.HandLeft];
//Vector of Joints
Vector3D shoulder = new Vector3D(shoulder_j.Position.X, shoulder_j.Position.Y, shoulder_j.Position.Z);
Vector3D elbow = new Vector3D(elbow_j.Position.X, elbow_j.Position.Y, elbow_j.Position.Z);
Vector3D hand = new Vector3D(hand_j.Position.X, hand_j.Position.Y, hand_j.Position.Z);
// Vektorberechnung
return(AngleCalculation.getAngle(shoulder, elbow, hand));
}
public static float getShoulderRoll_Left(Skeleton skeleton)
{
// Get fitting joints
Joint hip_left_j = skeleton.Joints[JointType.HipLeft];
Joint hip_right_j = skeleton.Joints[JointType.HipRight];
Joint shoulder_j = skeleton.Joints[JointType.ShoulderLeft];
Joint elbow_j = skeleton.Joints[JointType.ElbowLeft];
//Vector of Joints
Vector3D hip_left = new Vector3D(hip_left_j.Position.X, hip_left_j.Position.Y, hip_left_j.Position.Z);
Vector3D hip_right = new Vector3D(hip_right_j.Position.X, hip_right_j.Position.Y, hip_right_j.Position.Z);
Vector3D shoulder = new Vector3D(shoulder_j.Position.X, shoulder_j.Position.Y, shoulder_j.Position.Z);
Vector3D elbow = new Vector3D(elbow_j.Position.X, elbow_j.Position.Y, elbow_j.Position.Z);
// Vektorberechnung
return(AngleCalculation.getAngle(hip_left, hip_right, shoulder, elbow));
}
// This method will be called when the thread is started.
public void DoWork()
{
float shoulderPitch_Right = 0.0f;
float shoulderRoll_Right = 0.0f;
float elbowRoll_Right = 0.0f;
float elbowYaw_Right = 0.0f;
float shoulderPitch_Left = 0.0f;
float shoulderRoll_Left = 0.0f;
float elbowRoll_Left = 0.0f;
float elbowYaw_Left = 0.0f;
while (!_shouldStop)
{
if (currentSkeleton == null)
{
continue;
}
//Hier werden Berechnungen mit CURRENTSKELETON (EVTL PUFFER!!!) getätigt
//BERECHNE WINKEL a-d
//updateAngles
//Sleep
//Zuverlässigkeit der Joints
//filter.addCurrentArmPos()
//Right
shoulderPitch_Right = AngleCalculation.getShoulderPitch_Right(currentSkeleton);
shoulderRoll_Right = AngleCalculation.getShoulderRoll_Right(currentSkeleton);
elbowRoll_Right = AngleCalculation.getElbowRoll_Right(currentSkeleton);
elbowYaw_Right = AngleCalculation.getElbowYaw_Right(currentSkeleton);
//Left
shoulderPitch_Left = AngleCalculation.getShoulderPitch_Left(currentSkeleton);
shoulderRoll_Left = AngleCalculation.getShoulderRoll_Left(currentSkeleton);
elbowRoll_Left = AngleCalculation.getElbowRoll_Left(currentSkeleton);
elbowYaw_Left = AngleCalculation.getElbowYaw_Left(currentSkeleton);
filter.addCurrentArmPos(
new ArmAngles(
shoulderPitch_Right,
shoulderRoll_Right,
elbowRoll_Right,
elbowYaw_Right,
shoulderPitch_Left,
shoulderRoll_Left,
elbowRoll_Left,
elbowYaw_Left)
);
ArmAngles filtered_values = filter.getArmValue();
//skeletonAngleHandler.updateAngles(shoulderPitch_Right, shoulderRoll_Right, elbowRoll_Right, elbowYaw_Right, shoulderPitch_Left, shoulderRoll_Left, elbowRoll_Left, elbowYaw_Left);
skeletonAngleHandler.updateAngles(
filtered_values.shoulderPitch_Right, filtered_values.shoulderRoll_Right, filtered_values.elbowRoll_Right, filtered_values.elbowYaw_Right,
filtered_values.shoulderPitch_Left, filtered_values.shoulderRoll_Left, filtered_values.elbowRoll_Left, filtered_values.elbowYaw_Left);
Thread.Sleep(100);
}
Console.WriteLine("Angle Calculation shutted down");
}
