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
|