(*

ISO 10303 TC184/SC4/WG12 N8482

EXPRESS Source:
ISO 10303-105 ed2 Kinematics - Kinematic state schema

The following permission notice and disclaimer shall be included in all copies of this EXPRESS schema ("the Schema"), 
and derivations of the Schema:

Copyright ISO 2014  All rights reserved
Permission is hereby granted, free of charge in perpetuity, to any person obtaining a copy of the Schema,
to use, copy, modify, merge and distribute free of charge, copies of the Schema for the purposes of developing, 
implementing, installing and using software based on the Schema, and to permit persons to whom the Schema is furnished to do so, 
subject to the following conditions:

THE SCHEMA IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, 
INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. 
IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, 
DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, 
ARISING FROM, OUT OF OR IN CONNECTION WITH THE SCHEMA OR THE 
USE OR OTHER DEALINGS IN THE SCHEMA.

In addition, any modified copy of the Schema shall include the following notice:

THIS SCHEMA HAS BEEN MODIFIED FROM THE SCHEMA DEFINED IN
ISO 10303-105 ed2 Kinematics - Kinematic state schema
AND SHOULD NOT BE INTERPRETED AS COMPLYING WITH THAT STANDARD
*)

SCHEMA kinematic_state_schema '{iso standard 10303 part(105) version(4) object(1) kinematic_state_schema(4)}';

REFERENCE FROM kinematic_structure_schema;

REFERENCE FROM geometry_schema
    (axis2_placement_3d,
     cartesian_transformation_operator_3d,
     curve,
     direction,
     geometric_representation_context,
     geometric_representation_item,
     normalise,
     point,
     point_on_curve,
     point_on_surface,
     surface,
     rectangular_trimmed_surface,
     trimmed_curve);

REFERENCE FROM measure_schema
    (conversion_based_unit,
     global_unit_assigned_context,
     length_measure,
     plane_angle_measure,
     si_prefix,
     si_unit,
     si_unit_name,
     unit);

REFERENCE FROM representation_schema
    (functionally_defined_transformation,
     item_defined_transformation,
     representation,
     representation_context,
     representation_item,
     representation_relationship,
     representation_relationship_with_transformation);

TYPE spatial_rotation = SELECT
  (ypr_rotation,
   rotation_about_direction);
END_TYPE;

TYPE ypr_rotation = ARRAY [ypr_index(yaw) : ypr_index(roll)]
                    OF plane_angle_measure;
END_TYPE;

TYPE ypr_enumeration = ENUMERATION OF
  (yaw,
   pitch,
   roll);
END_TYPE;

ENTITY mechanism_state_representation 
  SUBTYPE OF (representation);
  SELF\representation.items : SET[1:?] OF pair_value; 
  represented_mechanism : mechanism_representation;
DERIVE
  SELF\representation.context_of_items : geometric_representation_context := represented_mechanism.context_of_items;
END_ENTITY;

ENTITY pair_value
  ABSTRACT SUPERTYPE OF ( ONEOF (
	sliding_surface_pair_value,
	rolling_surface_pair_value,
	revolute_pair_value,
	prismatic_pair_value,
	screw_pair_value,
	cylindrical_pair_value,
	spherical_pair_value,
	sliding_curve_pair_value,
	rolling_curve_pair_value,
	gear_pair_value,
	rack_and_pinion_pair_value,
	universal_pair_value,
	planar_pair_value,
	unconstrained_pair_value,
	point_on_surface_pair_value,
	point_on_planar_curve_pair_value,
	low_order_kinematic_pair_value
  ))
  SUBTYPE OF (geometric_representation_item);
  applies_to_pair : kinematic_pair;
END_ENTITY;

ENTITY sliding_surface_pair_value
  SUBTYPE OF (pair_value);
  SELF\pair_value.applies_to_pair : sliding_surface_pair;
  actual_point_on_surface_1       : point_on_surface;
  actual_point_on_surface_2       : point_on_surface;
  actual_rotation                 : plane_angle_measure;
WHERE
  WR1: SELF\pair_value.applies_to_pair\surface_pair.surface_1 :=:
       actual_point_on_surface_1.basis_surface;
  WR2: SELF\pair_value.applies_to_pair\surface_pair.surface_2 :=:
       actual_point_on_surface_2.basis_surface;
END_ENTITY;

ENTITY rolling_surface_pair_value
  SUBTYPE OF (pair_value);
  SELF\pair_value.applies_to_pair : rolling_surface_pair;
  actual_point_on_surface         : point_on_surface;
  actual_rotation                 : plane_angle_measure;
WHERE
  WR1: SELF\pair_value.applies_to_pair\surface_pair.surface_1 :=:
       actual_point_on_surface.basis_surface;
END_ENTITY;

ENTITY revolute_pair_value
  SUBTYPE OF (pair_value);
  SELF\pair_value.applies_to_pair : revolute_pair;
  actual_rotation                 : plane_angle_measure ;
END_ENTITY;

ENTITY prismatic_pair_value
  SUBTYPE OF (pair_value);
  SELF\pair_value.applies_to_pair : prismatic_pair;
  actual_translation              : length_measure;
END_ENTITY;

ENTITY screw_pair_value
  SUBTYPE OF (pair_value);
  SELF\pair_value.applies_to_pair : screw_pair;
  actual_rotation                 : plane_angle_measure;
DERIVE
  actual_translation              : length_measure
                                  := SELF\pair_value.applies_to_pair\
                                     screw_pair.pitch *
                                     plane_angle_for_pair_in_radian
                                      (SELF\pair_value.applies_to_pair,
                                       actual_rotation) / (2 * PI);
END_ENTITY;

ENTITY cylindrical_pair_value
  SUBTYPE OF (pair_value);
  SELF\pair_value.applies_to_pair : cylindrical_pair;
  actual_translation              : length_measure;
  actual_rotation                 : plane_angle_measure;
END_ENTITY;

ENTITY low_order_kinematic_pair_value
  SUBTYPE OF (pair_value);
  SELF\pair_value.applies_to_pair : low_order_kinematic_pair;
  actual_translation_x            : length_measure;
  actual_translation_y            : length_measure;
  actual_translation_z            : length_measure;
  actual_rotation_x               : plane_angle_measure;
  actual_rotation_y               : plane_angle_measure;
  actual_rotation_z               : plane_angle_measure;
END_ENTITY;

TYPE spherical_pair_select = SELECT (
  spherical_pair,
  spherical_pair_with_pin);
END_TYPE;  

ENTITY spherical_pair_value
  SUBTYPE OF (pair_value);
  SELF\pair_value.applies_to_pair : spherical_pair_select;
  input_orientation               : spatial_rotation;
DERIVE
  actual_orientation              : ypr_rotation
                                  := convert_spatial_to_ypr_rotation
                                     (SELF\pair_value.applies_to_pair,
                                      input_orientation);
END_ENTITY;

ENTITY sliding_curve_pair_value
  SUBTYPE OF (pair_value);
  SELF\pair_value.applies_to_pair : sliding_curve_pair;
  actual_point_on_curve_1         : point_on_curve;
  actual_point_on_curve_2         : point_on_curve;
WHERE
  WR1: SELF\pair_value.applies_to_pair\planar_curve_pair.curve_1 :=:
       actual_point_on_curve_1.basis_curve;
  WR2: SELF\pair_value.applies_to_pair\planar_curve_pair.curve_2 :=:
       actual_point_on_curve_2.basis_curve;
END_ENTITY;

ENTITY rolling_curve_pair_value
  SUBTYPE OF (pair_value);
  SELF\pair_value.applies_to_pair : rolling_curve_pair;
  actual_point_on_curve_1         : point_on_curve;
WHERE
  WR1: SELF\pair_value.applies_to_pair\planar_curve_pair.curve_1 :=:
       actual_point_on_curve_1.basis_curve;
END_ENTITY;

ENTITY gear_pair_value
  SUBTYPE OF (pair_value);
  SELF\pair_value.applies_to_pair : gear_pair;
  actual_rotation_1               : plane_angle_measure;
DERIVE
  actual_rotation_2               : plane_angle_measure
                                  := - actual_rotation_1 *
                                     SELF\pair_value.applies_to_pair\
                                     gear_pair.gear_ratio;
END_ENTITY;

ENTITY rack_and_pinion_pair_value
  SUBTYPE OF (pair_value);
  SELF\pair_value.applies_to_pair : rack_and_pinion_pair;
  actual_displacement             : length_measure;
DERIVE
  actual_rotation : plane_angle_measure
   := 0.0; (*
                  := convert_plane_angle_for_pair_from_radian
                     (SELF\pair_value.applies_to_pair,
                      (- actual_displacement /
                       SELF\pair_value.applies_to_pair\
                       rack_and_pinion_pair.pinion_radius)); *)
END_ENTITY;

ENTITY universal_pair_value
  SUBTYPE OF (pair_value);
  SELF\pair_value.applies_to_pair : universal_pair;
  first_rotation_angle            : plane_angle_measure;
  second_rotation_angle           : plane_angle_measure;
END_ENTITY;

ENTITY planar_pair_value
  SUBTYPE OF (pair_value);
  SELF\pair_value.applies_to_pair : planar_pair;
  actual_rotation                 : plane_angle_measure;
  actual_translation_x            : length_measure;
  actual_translation_y            : length_measure;
END_ENTITY;

ENTITY unconstrained_pair_value
  SUBTYPE OF (pair_value);
  SELF\pair_value.applies_to_pair : unconstrained_pair;
  actual_placement                : axis2_placement_3d;
END_ENTITY;

ENTITY point_on_surface_pair_value
  SUBTYPE OF (pair_value);
  SELF\pair_value.applies_to_pair : point_on_surface_pair;
  actual_point_on_surface         : point_on_surface;
  input_orientation               : spatial_rotation;
DERIVE
  actual_orientation              : ypr_rotation
                                  := convert_spatial_to_ypr_rotation
                                     (SELF\pair_value.applies_to_pair,
                                      input_orientation);
WHERE
  WR1: SELF\pair_value.applies_to_pair\point_on_surface_pair.pair_surface :=:
       actual_point_on_surface.basis_surface;
END_ENTITY;

ENTITY point_on_planar_curve_pair_value
  SUBTYPE OF (pair_value);
  SELF\pair_value.applies_to_pair : point_on_planar_curve_pair;
  actual_point_on_curve           : point_on_curve;
  input_orientation               : spatial_rotation;
DERIVE
  actual_orientation              : ypr_rotation
                                  := convert_spatial_to_ypr_rotation
                                     (SELF\pair_value.applies_to_pair,
                                      input_orientation);
WHERE
  WR1: SELF\pair_value.applies_to_pair\point_on_planar_curve_pair.pair_curve
       :=: actual_point_on_curve.basis_curve;
END_ENTITY;

ENTITY rotation_about_direction
  SUBTYPE OF (geometric_representation_item); -- LK
  direction_of_axis : direction;
  rotation_angle    : plane_angle_measure;
WHERE
  WR1: SIZEOF (direction_of_axis.direction_ratios) = 3;
END_ENTITY;

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 NOT('MEASURE_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);

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

FUNCTION ypr_index (ypr : ypr_enumeration) : INTEGER;
  CASE ypr OF
    yaw    : RETURN (1);
    pitch  : RETURN (2);
    roll   : RETURN (3);
  END_CASE;
  RETURN (?);
END_FUNCTION;


SUBTYPE_CONSTRAINT kss_geometric_representation_item_subtypes FOR geometric_representation_item;
  ONEOF (
         rotation_about_direction,
         su_parameters);
END_SUBTYPE_CONSTRAINT;


END_SCHEMA;