/// <summary>
/// Calcula a lei dos cossenos em todos os planos
/// </summary>
/// <param name="subPose">SubPose que contém a posição dos joints</param>
/// <returns>Retorna um objeto que contém o resultado do angulo em cada um dos planos 2D utilizando a lei dos cossenos</returns>
internal static LawOfCosinesResult LawOfCosinesCalculation( SubPose subPose )
{
LawOfCosinesResult lawOfCosinesResult = new LawOfCosinesResult();
lawOfCosinesResult.PlanXY = LawOfCosinesPlanCalculation( subPose.AuxiliaryJoint1.Position, subPose.CenterJoint.Position, subPose.AuxiliaryJoint2.Position, Axis.X, Axis.Y );
lawOfCosinesResult.PlanXZ = LawOfCosinesPlanCalculation( subPose.AuxiliaryJoint1.Position, subPose.CenterJoint.Position, subPose.AuxiliaryJoint2.Position, Axis.X, Axis.Z );
lawOfCosinesResult.PlanYZ = LawOfCosinesPlanCalculation( subPose.AuxiliaryJoint1.Position, subPose.CenterJoint.Position, subPose.AuxiliaryJoint2.Position, Axis.Y, Axis.Z );
return lawOfCosinesResult;
}
/// <summary>
/// Calcula o produto escalar dos jointts da subpose em um sistema de coordenadas 3D
/// </summary>
/// <param name="subPose">SubPose que contém a posição dos joints</param>
/// <returns>Returna o valor do cálculo do produto escalar</returns>
internal static double ScalarProductCalculation( SubPose subPose )
{
Vector3D vectorV = Vector3DBetweenTwoJoints(subPose.AuxiliaryJoint1,subPose.CenterJoint);
Vector3D vectorW = Vector3DBetweenTwoJoints(subPose.AuxiliaryJoint2,subPose.CenterJoint);
double resultVW = vectorV.X * vectorW.X + vectorV.Y * vectorW.Y + vectorV.Z * vectorW.Z;
double resultModuleVW = Math.Sqrt( Math.Pow( vectorV.X, 2 ) + Math.Pow( vectorV.Y, 2 ) + Math.Pow( vectorV.Z, 2 ) ) * Math.Sqrt( Math.Pow( vectorW.X, 2 ) + Math.Pow( vectorW.Y, 2 ) + Math.Pow( vectorW.Z, 2 ) );
double resultInRadians = Math.Acos( resultVW / resultModuleVW );
double resultInDegrees = resultInRadians * 180 / Math.PI;
return resultInDegrees;
}
/// <summary>
/// Método para comprar o esqueleto corrente com a subpose enviada por parâmetro
/// </summary>
/// <param name="userSkeleton">Esqueleto do usuário em tempo de execução</param>
/// <param name="subPoseForTest">SubPose para comparar</param>
/// <param name="recognitionType">Indica o algortimo que será utilizado para comparação</param>
/// <returns>Retorna true caso a subpose seja reconhecida, caso contrário retorna false</returns>
private bool SubPoseTest(SubPose subPoseForTest, Skeleton userSkeleton, PoseRecognitionType recognitionType)
{
Joint centerJoint = userSkeleton.Joints[subPoseForTest.CenterJoint.JointType];
Joint auxiliaryJoint1 = userSkeleton.Joints[subPoseForTest.AuxiliaryJoint1.JointType];
Joint auxiliaryJoint2 = userSkeleton.Joints[subPoseForTest.AuxiliaryJoint2.JointType];
SubPose userSubPose = new SubPose(centerJoint, auxiliaryJoint1, auxiliaryJoint2);
bool returnValue = true;
if (recognitionType == PoseRecognitionType.scalarProduct)
{
double scalarProductResult = MathFunctionsManager.ScalarProductCalculation(userSubPose);
returnValue = MathFunctionsManager.ComparisonWithErrorMargin(scalarProductResult, subPoseForTest.Angle.ScalarProductResult, subPoseForTest.AngleMarginError);
}
else
{
LawOfCosinesResult lawOfCosinesResult = MathFunctionsManager.LawOfCosinesCalculation(userSubPose);
returnValue &= MathFunctionsManager.ComparisonWithErrorMargin(subPoseForTest.Angle.LawOfCosinesResult.PlanXY, lawOfCosinesResult.PlanXY, subPoseForTest.AngleMarginError);
returnValue &= MathFunctionsManager.ComparisonWithErrorMargin(subPoseForTest.Angle.LawOfCosinesResult.PlanXZ, lawOfCosinesResult.PlanXZ, subPoseForTest.AngleMarginError);
returnValue &= MathFunctionsManager.ComparisonWithErrorMargin(subPoseForTest.Angle.LawOfCosinesResult.PlanYZ, lawOfCosinesResult.PlanYZ, subPoseForTest.AngleMarginError);
}
return returnValue;
}
