Calculate angle between two lines (which are represent human arm) using Kinect data set in Java

Question

I have obtained the coordinates of a human shoulder, elbow and wrist using Microsoft Kinect device (data are taken with respect to the Kinect Device). While taking coordinates I'm moved the arm up and down. sample data set can be show as follows.

7/7/2015 12:02:49 AM shoulder X -0.06475954 shoulder Y 0.266225 shoulder Z 1.414332 elbow X 0.002287441 elbow Y 0.03676218 elbow Z 1.424792 wrist X 0.1791002 wrist Y -0.06900118 wrist Z 1.455758

7/7/2015 12:02:49 AM shoulder X -0.06655618 shoulder Y 0.2654685 shoulder Z 1.413007 elbow X 0.001183244 elbow Y 0.0374795 elbow Z 1.424512 wrist X 0.1779053 wrist Y -0.06793896 wrist Z 1.4554

7/7/2015 12:02:49 AM shoulder X -0.0703955 shoulder Y 0.264899 shoulder Z 1.408783 elbow X -0.001478138 elbow Y 0.03802216 elbow Z 1.422277 wrist X 0.1769906 wrist Y -0.06481737 wrist Z 1.4514

Then I tried to calculate the angle in the arm in two conditions.

1. Assuming that the Z coordinates are fixed (actually above data set is taken such a way that this assumption is true)
2. Assuming that the Z coordinate are not fixed.

I used a java code to calculate the angles in above two conditions. Java code is as follows.

import java.io.*;

class main {

    public static void main(String args[]){
        try{
            FileInputStream fstream = new FileInputStream("C:\\Users\\Chathu\\Desktop\\project code\\src\\output.txt");
            DataInputStream in = new DataInputStream(fstream);
            BufferedReader br = new BufferedReader(new InputStreamReader(in));
            String strLine;
            while ((strLine = br.readLine()) != null)   {
                String[] tokens = strLine.split(" ");

                //time

                //shoulder cordinates
                double ShoulderX= Double.parseDouble(tokens[5]);
                double ShoulderY= Double.parseDouble(tokens[8]);
                double ShoulderZ= Double.parseDouble(tokens[11]);


                //elbow cordinates
                double ElbowX= Double.parseDouble(tokens[14]);
                double ElbowY= Double.parseDouble(tokens[17]);
                double ElbowZ= Double.parseDouble(tokens[20]);


                //wrist cordinates
                double WristX= Double.parseDouble(tokens[23]);
                double WristY= Double.parseDouble(tokens[26]);
                double WristZ= Double.parseDouble(tokens[29]);



                //calculate angle if Z is fixed

                double M1=((ShoulderY-ElbowY)/(ShoulderX-ElbowX));
                double M2=((WristY-ElbowY)/(WristX-ElbowX));

                double tanAlfa=Math.abs((M1-M2)/(1+(M1*M2)));

                System.out.println("Time : "+getTimeinInt(tokens[1]));
                System.out.println("2D angle: "+Math.toDegrees(Math.atan(tanAlfa)));



                //calculate andgle when Z is not fixed

                //Create 2 vectors
                double[] u={ShoulderX-ElbowX,ShoulderY-ElbowY,ShoulderZ-ElbowZ };
                double[] v={WristX-ElbowX,WristY-ElbowY,WristZ-ElbowZ };

                double absu=Math.sqrt(Math.pow(u[0],2)+Math.pow(u[1],2)+Math.pow(u[2],2));
                double absv=Math.sqrt(Math.pow(v[0],2)+Math.pow(v[1],2)+Math.pow(v[2],2));

                double cosTheata=((dotProd(u,v))/(absu*absv));

                System.out.println("3D angle: "+Math.acos(cosTheata));


            }
            in.close();
        }catch (Exception e){
            System.err.println("Error: " + e.getMessage());
        }
    }



    public static int getTimeinInt(String time){

        String[] tokens = time.split(":");
        int hours = Integer.parseInt(tokens[0]);
        int minutes = Integer.parseInt(tokens[1]);
        int seconds = Integer.parseInt(tokens[2]);
        int duration = 3600 * hours + 60 * minutes + seconds;

        return duration;
    }

    public static double dotProd(double[] a, double[] b){
        if(a.length != b.length){
            throw new IllegalArgumentException("The dimensions have to be equal!");
        }
        double sum = 0;
        for(int i = 0; i < a.length; i++){
            sum += a[i] * b[i];
        }
        return sum;
    }
}

But when i run this code and check the calculated angle values i got a totally different values under above two assumptions.

first 3 answers are as follows (answers correspond to above sample data set)

Time : 43369
2D angle: 42.82568606638748
3D angle: 2.3907040436551847
Time : 43369
2D angle: 42.63544266854971
3D angle: 2.3947689168198463
Time : 43369
2D angle: 43.151072486090776
3D angle: 2.387883634441205

Can anyone help me to find the problem there ?

Answer 1

Thank you all for your help. Finally I found the mistake in my java code. I use System.out.println("3D angle: "+Math.acos(cosTheata)); to print the angle between two vectors in 3D space. When i read the documentation of acos() method, it says this method returns the angle in radian . In 2D plane result I used Math.toDegrees() method to convert the angles into degrees. So all my 3D coordinates angle values are in radian.

Answer 2

You can find the angle between 2 lines with cosine.

wrist = point1
elbo = point2
soulder = point3

vector1 = point1 - point2 
vector2 = point1 - point3

Now that you have vectors 1 and 2 pointing from the elbow to the wrist and shoulder respectively the angle can be found by taking:

The arccos of the ratio of the dot product of these 2 vectors over the product of their magnitude.

For more information on the math here check out This page on Wikipedia

Which is what you have for "3d angle..."

You cannot use your atan approach because your elbow angle isn't 90. If you knew it was why would you be looking for it?

I'm really not sure exactly what it is you are going for with "2d angle..."