计算关节夹角

计算关节夹角

float calculateAngle(float startX,float startY,float startZ,
    float centerX,float centerY,float centerZ,
    float endX,float endY,float endZ)
{
    float jointAngleValue = 0.0f;

    float vector1_x = startX - centerX;
    float vector1_y = startY - centerY;
    float vector1_z = startZ - centerZ;

    float vector2_x = endX - centerX;
    float vector2_y = endY - centerY;
    float vector2_z = endZ - centerZ;

    //cos  =  (vector1*vector2)/(mul(vector1)*mul(vector2))

    jointAngleValue = ((vector1_x*vector2_x + vector1_y*vector2_y + vector1_z*vector2_z) / (sqrt(vector1_x*vector1_x + vector1_y*vector1_y + vector1_z*vector1_z)*sqrt(vector2_x*vector2_x + vector2_y*vector2_y + vector2_z*vector2_z)));
    float jointAngleValue2 = acos(jointAngleValue);

    return jointAngleValue2;
}

 

float jointAngle8_9_10_Value = calculateAngle(
        0.871856,0.151206,4.01117,
        1.17578,0.359172,3.95764,
        1.01659, 0.577535, 3.94001
        );

    std::cout << "jointAngle8_9_10_Value:" << jointAngle8_9_10_Value*180.0 / 3.1415926 << std::endl;

 

#################