Finding Elbow Angle with Kinect

Question

Vector3D Rwrist = new Vector3D(skel.Joints[JointType.WristRight].Position.X,
            skel.Joints[JointType.WristRight].Position.Y, skel.Joints[JointType.WristRight].Position.Z);
        Vector3D Relbow = new Vector3D(skel.Joints[JointType.ElbowRight].Position.X,
            skel.Joints[JointType.ElbowRight].Position.Y, skel.Joints[JointType.ElbowRight].Position.Z);
        Vector3D Rshoulder = new Vector3D(skel.Joints[JointType.ShoulderRight].Position.X,
            skel.Joints[JointType.ShoulderRight].Position.Y, skel.Joints[JointType.ShoulderRight].Position.Z);

        Vector3D wristToElbow = Vector3D.Subtract(Rwrist, Relbow);
        Vector3D elbowToShoulder = Vector3D.Subtract(Relbow, Rshoulder);

        elbowAngle = Vector3D.AngleBetween(elbowToShoulder, wristToElbow);

So it keeps returning NaN? I would really appreciate some help :) thanks so much guys!!

Answer 1

You have to normalize your Vectors. Try adding this code. I recommend that you do the AngleBetweenVector method yourself (See code below).

public class Angles
    {
    public double AngleBetweenTwoVectors(Vector3D vectorA, Vector3D vectorB)
        {
            double dotProduct;
            vectorA.Normalize();
            vectorB.Normalize();
            dotProduct = Vector3D.DotProduct(vectorA, vectorB);

            return (double)Math.Acos(dotProduct)/Math.PI*180;
        }

        public byte[] GetVector(Skeleton skeleton)
        {
            Vector3D RightShoulder = new Vector3D(skeleton.Joints[JointType.ShoulderRight].Position.X, skeleton.Joints[JointType.ShoulderRight].Position.Y, skeleton.Joints[JointType.ShoulderRight].Position.Z);
            Vector3D LeftShoulder = new Vector3D(skeleton.Joints[JointType.ShoulderLeft].Position.X, skeleton.Joints[JointType.ShoulderLeft].Position.Y, skeleton.Joints[JointType.ShoulderLeft].Position.Z);
            Vector3D RightElbow = new Vector3D(skeleton.Joints[JointType.ElbowRight].Position.X, skeleton.Joints[JointType.ElbowRight].Position.Y, skeleton.Joints[JointType.ElbowRight].Position.Z);
            Vector3D LeftElbow = new Vector3D(skeleton.Joints[JointType.ElbowLeft].Position.X, skeleton.Joints[JointType.ElbowLeft].Position.Y, skeleton.Joints[JointType.ElbowLeft].Position.Z);
            Vector3D RightWrist = new Vector3D(skeleton.Joints[JointType.WristRight].Position.X, skeleton.Joints[JointType.WristRight].Position.Y, skeleton.Joints[JointType.WristRight].Position.Z);
            Vector3D LeftWrist = new Vector3D(skeleton.Joints[JointType.WristLeft].Position.X, skeleton.Joints[JointType.WristLeft].Position.Y, skeleton.Joints[JointType.WristLeft].Position.Z);


            double AngleRightElbow = AngleBetweenTwoVectors(RightElbow - RightShoulder, RightElbow - RightWrist);
            double AngleLeftElbow = AngleBetweenTwoVectors(LeftElbow - LeftShoulder, LeftElbow - LeftWrist);

            byte[] Angles = {Convert.ToByte(AngleRightElbow), Convert.ToByte(AngleRightShoulder),Convert.ToByte(AngleLeftElbow),Convert.ToByte(AngleLeftShoulder)};
            return Angles;
        }
}

1. Define Vectors
2. Substract
3. Pass to AngleBetweenTwoVectors - method
4. (In AngleBetweenTwoVectors) normalize (divide by Vector length on every axis) for more information here
5. Calculate the dot product (For more here)
6. Use Arcosinus (Arcos) method
7. Product/PI*180, otherwise you would get the radian