Algorithm 无法获得用户相对于Kinect的正确方向

我正在项目中使用Microsoft Kinect。我必须完成的任务之一是通过Kinect传感器找到用户的方向（当用户转动时，方向会改变） 为此，我试图找到连接肩部的线与Kinect的x轴的夹角

我已经想出了下面的代码，但它给了我非常小的角度值，即使当我转向大约40度

double vector_x=skel.SkeletonPositions[NUI_SKELETON_POSITION_SHOULDER_LEFT].x-skel.SkeletonPositions[NUI_SKELETON_POSITION_SHOULDER_RIGHT].x;
    double vector_y=skel.SkeletonPositions[NUI_SKELETON_POSITION_SHOULDER_LEFT].y-skel.SkeletonPositions[NUI_SKELETON_POSITION_SHOULDER_RIGHT].y;
    double vector_z=skel.SkeletonPositions[NUI_SKELETON_POSITION_SHOULDER_LEFT].z-skel.SkeletonPositions[NUI_SKELETON_POSITION_SHOULDER_RIGHT].z;
    double len1=sqrtf(vector_x * vector_x + vector_y * vector_y + vector_z * vector_z);
    double vector_x1=1.0;
    double vector_y1=0.0;
    double vector_z1=0.0; 
    double len2=sqrtf(vector_x1 * vector_x1 + vector_y1 * vector_y1 + vector_z1 * vector_z1);
    double dot_product = vector_x * vector_x1 + vector_y * vector_y1 + vector_z * vector_z1; 
    double angle = dot_product / (len1 * len2); 
    coor_left=Convert(vector_x)+"\t"+Convert(vector_y)+"\t"+Convert(vector_z)+"\n";
OutputDebugStringA(Convert(acos(angle)).c_str());

当我添加弧度到度的转换时

double angle1=angle*180.0/3.14;

我得到的值从-33到-57（当我面对Kinect时），然后再到-33

---

但实际上，它应该是负的，然后是0，然后是正的。我哪里出错了？

我自己解决了。我意识到我在寻找错误向量之间的角度

我所需要做的就是把左肩和右肩在x-z平面上的投影，然后把问题简化为找到平面上两个向量之间的角度

以下是我所做的：

double CalcAngle(double p1x,double p1y, double p2x,double p2y, double p3x,double p3y, double p4x,double p4y)
{
//
// calculate the angle between the line from p1 to p2
// and the line from p3 to p4
//
double x1 = p1x - p2x;
double y1 = p1y - p2y;
double x2 = p3x - p4x;
double y2 = p3y - p4y;
//
double angle1 , angle2 , angle;
//
if (x1 != 0.0f)
angle1 = atan(y1/x1);
else
angle1 = 3.14159 / 2.0;  // 90 degrees
//
if (x2 != 0.0f)
angle2 = atan(y2/x2);
else
angle2 = 3.14159 / 2.0;  // 90 degrees
//
angle = fabs(angle2-angle1);
angle = angle * 180.0 / 3.14159;    // convert to degrees ???
//
return angle;
}

double myangle=CalcAngle(skel.SkeletonPositions[NUI_SKELETON_POSITION_SHOULDER_LEFT].x,skel.SkeletonPositions[NUI_SKELETON_POSITION_SHOULDER_LEFT].z,
        skel.SkeletonPositions[NUI_SKELETON_POSITION_SHOULDER_RIGHT].x,skel.SkeletonPositions[NUI_SKELETON_POSITION_SHOULDER_RIGHT].z,
        0,0,1,0);
        OutputDebugStringA(Convert(myangle).c_str());
    OutputDebugStringA("\n");

---

我自己解决了。我意识到我在寻找错误向量之间的角度

我所需要做的就是把左肩和右肩在x-z平面上的投影，然后把问题简化为找到平面上两个向量之间的角度

以下是我所做的：

double CalcAngle(double p1x,double p1y, double p2x,double p2y, double p3x,double p3y, double p4x,double p4y)
{
//
// calculate the angle between the line from p1 to p2
// and the line from p3 to p4
//
double x1 = p1x - p2x;
double y1 = p1y - p2y;
double x2 = p3x - p4x;
double y2 = p3y - p4y;
//
double angle1 , angle2 , angle;
//
if (x1 != 0.0f)
angle1 = atan(y1/x1);
else
angle1 = 3.14159 / 2.0;  // 90 degrees
//
if (x2 != 0.0f)
angle2 = atan(y2/x2);
else
angle2 = 3.14159 / 2.0;  // 90 degrees
//
angle = fabs(angle2-angle1);
angle = angle * 180.0 / 3.14159;    // convert to degrees ???
//
return angle;
}

double myangle=CalcAngle(skel.SkeletonPositions[NUI_SKELETON_POSITION_SHOULDER_LEFT].x,skel.SkeletonPositions[NUI_SKELETON_POSITION_SHOULDER_LEFT].z,
        skel.SkeletonPositions[NUI_SKELETON_POSITION_SHOULDER_RIGHT].x,skel.SkeletonPositions[NUI_SKELETON_POSITION_SHOULDER_RIGHT].z,
        0,0,1,0);
        OutputDebugStringA(Convert(myangle).c_str());
    OutputDebugStringA("\n");