Calcul d'angle machine avec IJK

Voici un exemple de code utilisé pour le calcul des angles machines en fonction de IJK dans mes sources sur Winpost :

'Calcul des angles machine avec machine tête twist A/C

 Function Calcul_A_C(IJK As Point3, PosAng As PosAngulaire) As PosAngulaire

    Dim Flip As Boolean

    If IJK.Y = 0 And IJK.X = 0 Then

       Calcul_A_C.A = 0

       Calcul_A_C.C = 0

    Else

        Calcul_A_C.C = Atan2(IJK.Y, IJK.X) * RADTODEG

        If Abs(PosAng.C - Calcul_A_C.C) >= 180 Then

            Flip = True

            Calcul_A_C.C = Calcul_A_C.C - 180

        Else

            Flip = False

        End If

       Calcul_A_C.A = Atan2(IJK.Z, Sqr(IJK.Y * IJK.Y + IJK.X * IJK.X)) * RADTODEG

       If Flip Then

            Calcul_A_C.A = -Calcul_A_C.A

       End If

    End If

 End Function

 'Calcul des angles machine avec machine tête twist B/C

 Function Calcul_B_C(IJK As Point3, PosAng As PosAngulaire) As PosAngulaire

 Dim Flip As Boolean

     If IJK.Y = 0 And IJK.X = 0 Then

       ' Tete Vertical

       Calcul_B_C.B = 0

       Calcul_B_C.C = 0

    Else

        Calcul_B_C.C = Atan2(IJK.X, IJK.Y) * RADTODEG

        If Abs(PosAng.C - Calcul_B_C.C) >= 180 Then

        Flip = True

            Calcul_B_C.C = Calcul_B_C.C - 180

        Else

            Flip = False

        End If

        Calcul_B_C.B = Atan2(IJK.Z, Sqr(IJK.Y * IJK.Y + IJK.X * IJK.X)) * RADTODEG

        If Flip Then

            Calcul_B_C.B = -Calcul_B_C.B

        End If

      End If

 End Function

 

  'Calcul des angles machine avec machine plateau A/C

 Function Calcul_A_C_Plateau(IJK As Point3, PosAng As PosAngulaire) As PosAngulaire

    If IJK.Y = 0 And IJK.X = 0 Then

       Calcul_A_C_Plateau.A = 0

       Calcul_A_C_Plateau.C = 0

    Else

       If IJK.X = 0 Then

            If IJK.Y > 0 Then

                Calcul_A_C_Plateau.C = -90

            Else

                Calcul_A_C_Plateau.C = 90

            End If

       Else

            Calcul_A_C_Plateau.C = -Atan2(IJK.Y, IJK.X) * RADTODEG

       End If

       Calcul_A_C_Plateau.A = -Atan2(IJK.Z, Sqr(IJK.Y * IJK.Y + IJK.X * IJK.X)) * RADTODEG

 

    End If

 End Function

 

 'Calcul des angles machine avec machine plateau B/C

 Function Calcul_B_C_Plateau(IJK As Point3, PosAng As PosAngulaire) As PosAngulaire

    If IJK.Y = 0 And IJK.X = 0 Then

       Calcul_B_C_Plateau.B = 0

       Calcul_B_C_Plateau.C = 0

    Else

       If IJK.Y = 0 Then

            If IJK.X > 0 Then

                Calcul_B_C_Plateau.C = -90

            Else

                Calcul_B_C_Plateau.C = 90

            End If

       Else

            Calcul_B_C_Plateau.C = -Atan2(IJK.X, IJK.Y) * RADTODEG

       End If

       Calcul_B_C_Plateau.B = -Atan2(Sqr(IJK.Y * IJK.Y + IJK.X * IJK.X), IJK.Z) * RADTODEG

    End If

 End Function