After carefully examining the Kinect SDK code, I have found the function which finds the start and end positions of the joints.

The function ‘public override void Draw’, is the function that draws the bone frame using the skeleton joints, and then this passes the jointID data to the function ‘public void DrawBone’ which is the function that calculates the start and end positions of X and Y for each jointID.

Looking at the function ‘public override void Draw’ I have found the code which finds the difference of the start X and start Y, and end X and end Y positions of the joint, then calculates the angle of these differences using ‘Math.Atan2’.

public void DrawBone(JointCollection joints, JointType startJoint, JointType endJoint)
{
Vector2 start = this.mapMethod(joints[startJoint].Position);
Vector2 end = this.mapMethod(joints[endJoint].Position);

Vector2 diff = end – start;

Vector2 scale = new Vector2(1.0f, diff.Length() / this.boneTexture.Height);

float angle = (float)Math.Atan2(diff.Y, diff.X) – MathHelper.PiOver2;   // the code to calculate angles using vector2 diff

There are a few minor tweaks to be made, which I will implement to change this to a vector 3 and pass the jointID data to it in order to calculate the angles on a vector. This will make it easier for the robot to move to different angle positions because everytime it has moved to a certain position, the angle will be set back to zero so then that vector becomes the new start position, and using vector 3 this can be implemented.

——————————————————————

RE-WRITING THE XNA CODE TO DISPLAY ANGLES CORRECTLY:

I decided to rewrite the code using vector 3 because the vector3 needs to be initiated so you can pass three different joints to it in order to work out the angle for them joints using hte three positions (triangle). This meant that a new function similar to the one above has to be made where the joints are passed through to the vectors.

 

    public void anglejoints(JointCollection joints, JointType j1, JointType j2, JointType j3)
        {

            Vector3 a = new Vector3(0, joints[j1].Position.Y – joints[j2].Position.Y, joints[j1].Position.Z – joints[j2].Position.Z);
            Vector3 b = new Vector3(0, joints[j3].Position.Y – joints[j2].Position.Y, joints[j3].Position.Z – joints[j2].Position.Z);

            a.Normalize();
            b.Normalize();
            double dotProduct = Vector3.Dot(a, b);
            double angle = Math.Acos(dotProduct);
            angle = angle * 180 / Math.PI;

           // return angle;
            Console.WriteLine(angle);
        }

This is the code I wrote which allows the joints to be passed into vector3, and then the calculation can be made to work out the correct angle and then this prints to the console, but in the later stage I will make changes to the form window to display the angles on screen.

As well as this code, some changes had to be made to the ‘public override draw function’ and the the following was added:

 this.DrawBone(skeleton.Joints, JointType.ShoulderRight, JointType.ElbowRight);

 anglejoints(skeleton.Joints, JointType.ShoulderLeft, JointType.ElbowLeft, JointType.HandLeft);

this allows me to create a new object of the JointTypes and I can pass three joint types to the object. Then this object is passed to the function above public void anglejoints.

… All in all, it is now printing angle joints, LET THE DISSERTATION BEGIN!