Overview Schemas Index

KINEMATIC_STRUCTURE_SCHEMA (jsdai.SKinematic_structure_schema)


FUNCTION convert_spatial_to_ypr_rotation
          (pair : kinematic_pair, rotation : spatial_rotation) : ypr_rotation;

LOCAL
    axis       : direction;
    angle      : plane_angle_measure;   -- rotation angle IN  application
                                        -- specific units
    conv_angle : plane_angle_measure;   -- rotation angle IN  radians
    ya, pa, ra : plane_angle_measure;   -- yaw, pitch, AND  roll angle
    ucf        : REAL;                  -- unit conversion factor
    dx, dy, dz : REAL;                  -- components OF  direction vector
    s_a, c_a   : REAL;                  -- sine AND  cosine OF  rotation angle
    rotmat     : ARRAY  [1 : 3] OF
                 ARRAY [1 : 3] OF  REAL; -- rotation matrix
    cm1        : REAL;
    s_y, c_y   : REAL;
    s_r, c_r   : REAL;
  END_LOCAL;

  -- IF  rotation is already a ypr_rotation, RETURN  it immediately
  IF  'KINEMATIC_STRUCTURE_SCHEMA.YPR_ROTATION' IN  TYPEOF (rotation) THEN
    RETURN  (rotation);
  END_IF;

  -- rotation is a rotation_about_direction

  axis  := normalise (rotation\rotation_about_direction.direction_of_axis);
  angle := rotation\rotation_about_direction.rotation_angle;

  -- a zero rotation is converted trivially
  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];

  -- provide angle measured IN  radian

  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);

  -- axis parallel either TO  x-axis OR  TO  z-axis?
  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
      -- axis parallel TO  x-axis - USE  x-axis AS  roll axis
      IF  (dx > 0.0) THEN
        RETURN  ([0.0, 0.0, ya]);
      ELSE
        RETURN  ([0.0, 0.0, ra]);
      END_IF;
    ELSE
      -- axis parallel TO  z-axis - USE  z-axis AS  yaw axis
      IF  (dz > 0.0) THEN
        RETURN  ([ya, 0.0, 0.0]);
      ELSE
        RETURN  ([ra, 0.0, 0.0]);
      END_IF;
    END_IF;
  END_IF;

  -- axis parallel TO  y-axis - USE y-axis AS pitch axis
  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;

  -- axis NOT  parallel TO  any axis OF  coordinate system
  -- compute rotation matrix

  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 ] ];

  -- rotmat[1][3] equals SIN  (pitch_angle)
  IF  (ABS (rotmat[1][3]) = 1.0) THEN
    -- |pa| = PI/2
    BEGIN
      IF  (rotmat[1][3] = 1.0) THEN
        pa := 0.5 * PI;
      ELSE
        pa := -0.5 * PI;
      END_IF;

      -- IN  this case, only the sum OR difference OF  roll AND  yaw angles
      -- is relevant AND  can be evaluated FROM the matrix.
      -- According TO  IP `rectangular pitch angle' for ypr_rotation,
      -- the roll angle is set to  zero.

      ra := 0.0;
      ya := ATAN  (rotmat[2][1], rotmat[2][2]);

      -- result of ATAN  is in the range [-PI/2, PI/2].
      -- Here all four quadrants are needed.

      IF  (rotmat[2][2] < 0.0) THEN
        IF  ya <= 0.0 THEN
          ya := ya + PI;
        ELSE
          ya := ya - PI;
        END_IF;
      END_IF;
    END;
  ELSE
    -- COS  (pitch_angle) not equal to zero
    BEGIN
      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;
  END_IF;

  ya := ya * ucf;
  pa := pa * ucf;
  ra := ra * ucf;

  RETURN ([ya, pa, ra]);

END_FUNCTION; -- convert_spatial_to_ypr_rotation

public class FConvert_spatial_to_ypr_rotation
          public static Value run(SdaiContext _context, Value pair, Value rotation)