/// <summary>
/// Calcula o angule entre os três joints enviados por parâmetros
/// </summary>
/// <param name="auxiliaryJointPosition">Posição do primeiro Joint</param>
/// <param name="centerJointPosition">Posição do segundo Joint</param>
/// <param name="auxiliary2JointPosition">Posição do terceiro Joint</param>
/// <param name="firstAxis">Primeiro eixo para o cálculo</param>
/// <param name="secondAxis">Segundo eixo para o cálculo</param>
/// <returns>Retorna o resultado da lei dos cossenos em graus</returns>
private static double LawOfCosinesPlanCalculation( SkeletonPoint auxiliaryJointPosition, SkeletonPoint centerJointPosition,SkeletonPoint auxiliary2JointPosition, Axis firstAxis, Axis secondAxis )
{
double firstAxisCenterJointPosition = centerJointPosition.GetType().GetProperty( firstAxis.ToString() ).GetValue( centerJointPosition, null ).To<double>();
double secondAxisCenterJointPosition = centerJointPosition.GetType().GetProperty(secondAxis.ToString()).GetValue(centerJointPosition, null).To<double>();
double firstAxisAuxiliaryJointPosition = auxiliaryJointPosition.GetType().GetProperty(firstAxis.ToString()).GetValue(auxiliaryJointPosition, null).To<double>();
double secondAxisAuxiliaryJointPosition = auxiliaryJointPosition.GetType().GetProperty(secondAxis.ToString()).GetValue(auxiliaryJointPosition, null).To<double>();
double firstAxisAuxiliaryJoint2Position = auxiliary2JointPosition.GetType().GetProperty(firstAxis.ToString()).GetValue(auxiliary2JointPosition, null).To<double>();
double secondAxisAuxiliaryJoint2Position = auxiliary2JointPosition.GetType().GetProperty(secondAxis.ToString()).GetValue(auxiliary2JointPosition, null).To<double>();
double a = StraightSegmentBetweenTwoJoints( firstAxisAuxiliaryJointPosition, secondAxisAuxiliaryJointPosition, firstAxisCenterJointPosition, secondAxisCenterJointPosition );
double b = StraightSegmentBetweenTwoJoints( firstAxisCenterJointPosition, secondAxisCenterJointPosition, firstAxisAuxiliaryJoint2Position, secondAxisAuxiliaryJoint2Position );
double c = StraightSegmentBetweenTwoJoints( firstAxisAuxiliaryJointPosition, secondAxisAuxiliaryJointPosition, firstAxisAuxiliaryJoint2Position, secondAxisAuxiliaryJoint2Position );
double resultInRadians = Math.Acos( ( Math.Pow( a, 2 ) + Math.Pow( b, 2 ) - Math.Pow( c, 2 ) ) / ( 2 * a * b ) );
double ResultInDegrees = resultInRadians * 180 / Math.PI;
return ResultInDegrees;
}
