Question

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

我在堆栈溢出中读到了一些代码，例如下面的代号，但是效果并不好，我不明白它们是如何不忽略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;
    }

Kinect关节角度

1 个答案: