FUNCTION derive_angle
(placement_1 : axis2_placement_3d, placement_2 : axis2_placement_3d) : plane_angle_measure;
LOCAL v1 : direction; v2 : direction; mag_v1 : REAL; mag_v2 : REAL; theta : plane_angle_measure; END_LOCAL; v1 := placement_1.p[1]; v2 := placement_2.p[1]; mag_v1 := SQRT (v1.direction_ratios[1]*v1.direction_ratios[1] + v1.direction_ratios[2]*v1.direction_ratios[2]); mag_v2 := SQRT (v2.direction_ratios[1]*v2.direction_ratios[1] + v2.direction_ratios[2]*v2.direction_ratios[2]); IF ((mag_v1 = 0.0) OR (mag_v2 = 0.0)) THEN theta := 0.0; RETURN (theta); END_IF; theta := ACOS ((v1.direction_ratios[1]*v2.direction_ratios[1] + v1.direction_ratios[2]*v2.direction_ratios[2]) / (mag_v1*mag_v2)); RETURN (theta); END_FUNCTION; -- derive_angle
|