FUNCTION convert_spatial_to_ypr_rotation

(* SCHEMA step_merged_ap_schema; *)

-- DIFF IN AP214
-- IN AP214/AP238 STEP-NC/AP242
FUNCTION convert_spatial_to_ypr_rotation
      (pair : kinematic_pair;
       rotation : spatial_rotation ) : ypr_rotation;
   LOCAL
      axis : direction;
      angle : plane_angle_measure;
      conv_angle : plane_angle_measure;
      ya : plane_angle_measure;
      pa : plane_angle_measure;
      ra : plane_angle_measure;
      ucf : REAL;
      dx : REAL;
      dy : REAL;
      dz : REAL;
      s_a : REAL;
      c_a : REAL;
      rotmat : ARRAY [1:3] OF ARRAY [1:3] OF REAL;
      cm1 : REAL;
      s_y : REAL;
      c_y : REAL;
      s_r : REAL;
      c_r : REAL;
   END_LOCAL;
      IF 'STEP_MERGED_AP_SCHEMA.YPR_ROTATION' IN TYPEOF(rotation) THEN
         RETURN (rotation);
      END_IF;
      axis := normalise(rotation\rotation_about_direction.direction_of_axis);
      angle := rotation\rotation_about_direction.rotation_angle;
      IF angle = 0.0 THEN
         RETURN ([ 0.0, 0.0, 0.0 ]);
      END_IF;
      dx := axis.direction_ratios[1];
      dy := axis.direction_ratios[2];
      dz := axis.direction_ratios[3];
      conv_angle := plane_angle_for_pair_in_radian(pair, angle);
      IF NOT ('STEP_MERGED_AP_SCHEMA.PLANE_ANGLE_MEASURE' IN TYPEOF(conv_angle)) THEN
         RETURN (?);
      END_IF;
      ucf := angle / conv_angle;
      s_a := SIN(conv_angle);
      c_a := COS(conv_angle);
      IF (dy = 0.0) AND (dx * dz = 0.0) THEN
         REPEAT WHILE conv_angle <= -3.14159;
            conv_angle := conv_angle + 2.0 * 3.14159;
         END_REPEAT;
         REPEAT WHILE conv_angle > 3.14159;
            conv_angle := conv_angle - 2.0 * 3.14159;
         END_REPEAT;
         ya := ucf * conv_angle;
         IF conv_angle <> 3.14159 THEN
            ra := -ya;
         ELSE
            ra := ya;
         END_IF;
         IF dx <> 0.0 THEN
            IF dx > 0.0 THEN
               RETURN ([ 0.0, 0.0, ya ]);
            ELSE
               RETURN ([ 0.0, 0.0, ra ]);
            END_IF;
         ELSE
            IF dz > 0.0 THEN
               RETURN ([ ya, 0.0, 0.0 ]);
            ELSE
               RETURN ([ ra, 0.0, 0.0 ]);
            END_IF;
         END_IF;
      END_IF;
      IF ((dy <> 0.0) AND (dx = 0.0)) AND (dz = 0.0) THEN
         IF c_a >= 0.0 THEN
            ya := 0.0;
            ra := 0.0;
         ELSE
            ya := ucf * 3.14159;
            ra := ya;
         END_IF;
         pa := ucf * ATAN(s_a, ABS(c_a));
         IF dy < 0.0 THEN
            pa := -pa;
         END_IF;
         RETURN ([ ya, pa, ra ]);
      END_IF;
      cm1 := 1.0 - c_a;
      rotmat := [ [ dx * dx * cm1 + c_a, dx * dy * cm1 - dz * s_a, dx * dz * cm1 + dy * s_a ], [ dx * dy * cm1 + dz * s_a, dy * dy * cm1 + c_a, dy * dz * cm1 - dx * s_a ], [ dx * dz * cm1 - dy * s_a, dy * dz * cm1 + dx * s_a, dz * dz * cm1 + c_a ] ];
      IF ABS(rotmat[1][3]) = 1.0 THEN
         BEGIN
            IF rotmat[1][3] = 1.0 THEN
               pa := 0.500000 * 3.14159;
            ELSE
               pa := -0.500000 * 3.14159;
            END_IF;
            ra := 0.0;
            ya := ATAN(rotmat[2][1], rotmat[2][2]);
            IF rotmat[2][2] < 0.0 THEN
               IF ya <= 0.0 THEN
                  ya := ya + 3.14159;
               ELSE
                  ya := ya - 3.14159;
               END_IF;
            END_IF;
         END;
      ELSE
         BEGIN
            ya := ATAN(-rotmat[1][2], rotmat[1][1]);
            IF rotmat[1][1] < 0.0 THEN
               IF ya <= 0.0 THEN
                  ya := ya + 3.14159;
               ELSE
                  ya := ya - 3.14159;
               END_IF;
            END_IF;
            ra := ATAN(-rotmat[2][3], rotmat[3][3]);
            IF rotmat[3][3] < 0.0 THEN
               IF ra <= 0.0 THEN
                  ra := ra + 3.14159;
               ELSE
                  ra := ra - 3.14159;
               END_IF;
            END_IF;
            s_y := SIN(ya);
            c_y := COS(ya);
            s_r := SIN(ra);
            c_r := COS(ra);
            IF ((ABS(s_y) > ABS(c_y)) AND (ABS(s_y) > ABS(s_r))) AND (ABS(s_y) > ABS(c_r)) THEN
               cm1 := -rotmat[1][2] / s_y;
            ELSE
               IF (ABS(c_y) > ABS(s_r)) AND (ABS(c_y) > ABS(c_r)) THEN
                  cm1 := rotmat[1][1] / c_y;
               ELSE
                  IF ABS(s_r) > ABS(c_r) THEN
                     cm1 := -rotmat[2][3] / s_r;
                  ELSE
                     cm1 := rotmat[3][3] / c_r;
                  END_IF;
               END_IF;
            END_IF;
            pa := ATAN(rotmat[1][3], cm1);
         END;
      END_IF;
      ya := ya * ucf;
      pa := pa * ucf;
      ra := ra * ucf;
      RETURN ([ ya, pa, ra ]);
END_FUNCTION;

Referenced By

Generated by STEP Tools® EXPRESS to HTML Converter
2024-09-06T14:00:33-04:00