FUNCTION convert_spatial_to_ypr_rotation

(* SCHEMA AUTOMOTIVE_DESIGN; *)

 
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 'AUTOMOTIVE_DESIGN.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 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 <= -PI;
      conv_angle := conv_angle + 2.0 * PI;
    END_REPEAT;
    REPEAT WHILE conv_angle > PI;
      conv_angle := conv_angle - 2.0 * PI;
    END_REPEAT;
    ya := ucf * conv_angle;
    IF conv_angle <> PI 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 * PI;
      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
    IF rotmat[1][3] = 1.0 THEN
      pa := 0.5 * PI;
    ELSE
      pa := -0.5 * PI;
    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 + PI;
      ELSE
        ya := ya - PI;
      END_IF;
    END_IF;
  ELSE
    ya := ATAN(-rotmat[1][2], rotmat[1][1]);
    IF rotmat[1][1] < 0.0 THEN
      IF ya <= 0.0 THEN
        ya := ya + PI;
      ELSE
        ya := ya - PI;
      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 + PI;
      ELSE
        ra := ra - PI;
      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_IF;
  ya := ya * ucf;
  pa := pa * ucf;
  ra := ra * ucf;
  RETURN ([ya, pa, ra]);
END_FUNCTION; -- 10303-105: kinematic_structure_schema

Referenced By

Generated by STEP Tools® EXPRESS to HTML Converter
2012-03-27T17:16:12-04:00