kinect - Kinect 关节角度

Question

我在 Kinect 中工作，我试图找到关节之间的角度，例如：Z 和 Y 坐标中的肘部角度。我发现这段代码用于计算 X 和 Y 坐标中的角度，但效果不佳，因为我不知道“旋转偏移”是什么。 http://www.embedded101.com/Blogs/JamesYWilson/tabid/70/entryid/167/Default.aspx

我读到了stackoverflow中的一些代码，例如波纹管，但效果不佳，我不明白他们如何不忽略z值。

/// <summary>
/// Return the angle between 3 Joints
/// Regresa el ángulo interno dadas 3 Joints
/// </summary>
/// <param name="j1"></param>
/// <param name="j2"></param>
/// <param name="j3"></param>
/// <returns></returns>
public static double AngleBetweenJoints(Joint j1, Joint j2, Joint j3)
{
    double Angulo = 0;
    double shrhX = j1.Position.X - j2.Position.X;
    double shrhY = j1.Position.Y - j2.Position.Y;
    double shrhZ = j1.Position.Z - j2.Position.Z;
    double hsl = vectorNorm(shrhX, shrhY, shrhZ);
    double unrhX = j3.Position.X - j2.Position.X;
    double unrhY = j3.Position.Y - j2.Position.Y;
    double unrhZ =j3.Position.Z - j2.Position.Z;
    double hul = vectorNorm(unrhX, unrhY, unrhZ);
    double mhshu = shrhX * unrhX + shrhY * unrhY + shrhZ * unrhZ;
    double x = mhshu / (hul * hsl);
    if (x != Double.NaN) 
    {
        if (-1 <= x && x <= 1)
        {
            double angleRad = Math.Acos(x);
            Angulo = angleRad *(180.0 / Math.PI);
        }
        else
            Angulo = 0;


    }
    else
        Angulo = 0;


    return Angulo;

}


/// <summary>
/// Euclidean norm of 3-component Vector
/// </summary>
/// <param name="x"></param>
/// <param name="y"></param>
/// <param name="z"></param>
/// <returns></returns>
private static double vectorNorm(double x, double y, double z)
{

    return Math.Sqrt(Math.Pow(x, 2) + Math.Pow(y, 2) + Math.Pow(z, 2));

}

请问有人可以帮助我吗

Answer 1

我找到了我正在寻找的方法:)

public static double myMethodZY(Joint j1, Joint j2, Joint j3)
    {
        Vector3 a = new Vector3(0, j1.Position.Y- j2.Position.Y, j1.Position.Z- j2.Position.Z);
        Vector3 b = new Vector3(0, j3.Position.Y - j2.Position.Y, j3.Position.Z - j2.Position.Z);
        a.Normalize();
        b.Normalize();
        double dotProduct = Vector3.Dot(a,b);
        double angle= Math.Acos(dotProduct);
        angle =  angle * 180 / Math.PI;
        //angle = 180 - angle;
        return angle;
    }