无人机转弯方式函数包附matlab代码.zip

%classdef BulbTurn < TurningStrategy % 灯泡转弯方式 % properties % end % methods % function obj = BulbTurn(theta, omega, r) % obj = obj@TurningStrategy(theta, omega, r); % end function [y, to_line] = turn(obj, from_line, to_line, dir) k = tan(obj.theta); from = from_line(end,:); to = to_line(1, :); line_from = [k, -1, from(2)-from(1)*k]; % 从起始点向下一条路径引垂线 foot1 = getFoot(line_from, to); line_to = [k, -1, to(2)-to(1)*k];% 从终止点向上一条路径引垂线 foot2 = getFoot(line_to, from); if ~onsegment(from_line, foot1) from = foot1; from_line = [from_line;from]; end if ~onsegment(to_line, foot2) to = foot2; to_line = [to;to_line]; end y = Line2D(from_line); theta_line = atan(-1/k); % 起点和终点连线与x轴的夹角 r = obj.r; omega = obj.omega; alpha = acos(0.5+omega/4/r); if dir if obj.theta < pi/2 arc1.radis = r; arc1.point = from - (to-from)*r/omega; arc1.startTheta=atan(-1/k)+pi-alpha; arc1.endTheta=atan(-1/k)+pi; arc2.radis = obj.r; arc2.point = (to+from)/2 + 2*r * sin(alpha)/ sqrt(1+k*k)*[1, k]; arc2.startTheta=atan(-1/k)-alpha; arc2.endTheta=atan(-1/k)+pi+alpha; arc3.radis = obj.r; arc3.point = to + (to-from)*obj.r/obj.omega; arc3.startTheta=atan(-1/k); arc3.endTheta=atan(-1/k)+alpha; elseif obj.theta <= pi arc1.radis = r; arc1.point = from - (to-from)*r/omega; arc1.startTheta=atan(-1/k)+pi-alpha; arc1.endTheta=atan(-1/k)+pi; arc2.radis = obj.r; arc2.point = (to+from)/2 - 2*r * sin(alpha)/ sqrt(1+k*k)*[1, k]; arc2.startTheta=atan(-1/k)-alpha; arc2.endTheta=atan(-1/k)+pi+alpha; arc3.radis = obj.r; arc3.point = to + (to-from)*obj.r/obj.omega; arc3.startTheta=atan(-1/k); arc3.endTheta=atan(-1/k)+alpha; elseif obj.theta <= pi*3/2 arc1.radis = r; arc1.point = from - (to-from)*r/omega; arc1.startTheta=atan(-1/k)+pi-alpha + pi; arc1.endTheta=atan(-1/k) + pi + pi; arc2.radis = obj.r; arc2.point = (to+from)/2 - 2*r * sin(alpha)/ sqrt(1+k*k)*[1, k]; arc2.startTheta=atan(-1/k)-alpha + pi; arc2.endTheta=atan(-1/k)+pi+alpha + pi; arc3.radis = obj.r; arc3.point = to + (to-from)*obj.r/obj.omega; arc3.startTheta=atan(-1/k) + pi; arc3.endTheta=atan(-1/k)+alpha + pi; else arc1.radis = r; arc1.point = from - (to-from)*r/omega; arc1.startTheta=atan(-1/k)+pi-alpha + pi + alpha; arc1.endTheta=atan(-1/k)+pi + pi + alpha; arc2.radis = obj.r; arc2.point = (to+from)/2 - 2*r * sin(alpha)/ sqrt(1+k*k)*[1, k]; arc2.startTheta=atan(-1/k)-alpha; arc2.endTheta=atan(-1/k)+pi+alpha; arc3.radis = obj.r; arc3.point = to + (to-from)*obj.r/obj.omega; arc3.startTheta=atan(-1/k) + pi - alpha; arc3.endTheta=atan(-1/k)+alpha + pi - alpha; end y = {y, Arc(arc1), Arc(arc2), Arc(arc3)}; else if obj.theta < pi/2 arc1.radis = r; arc1.point = from - (to-from)*r/omega; arc1.startTheta=atan(-1/k)+pi+alpha; arc1.endTheta=atan(-1/k)+pi; arc2.radis = obj.r; arc2.point = (to+from)/2 - 2*r * sin(alpha)/ sqrt(1+k*k)*[1, k]; arc2.startTheta=atan(-1/k)+pi-alpha; arc2.endTheta=atan(-1/k)+2*pi+alpha; arc3.radis = obj.r; arc3.point = to + (to-from)*obj.r/obj.omega; arc3.startTheta=atan(-1/k); arc3.endTheta=atan(-1/k)-alpha; elseif obj.theta <= pi arc1.radis = r; arc1.point = from - (to-from)*r/omega; arc1.startTheta=atan(-1/k)+pi+alpha; arc1.endTheta=atan(-1/k)+pi; arc2.radis = obj.r; arc2.point = (to+from)/2 + 2*r * sin(alpha)/ sqrt(1+k*k)*[1, k]; arc2.startTheta=atan(-1/k)+pi-alpha; arc2.endTheta=atan(-1/k)+2*pi+alpha; arc3.radis = obj.r; arc3.point = to + (to-from)*obj.r/obj.omega; arc3.startTheta=atan(-1/k); arc3.endTheta=atan(-1/k)-alpha; elseif obj.theta <= pi*3/2 arc1.radis = r; arc1.point = from - (to-from)*r/omega; arc1.startTheta=atan(-1/k)+alpha; arc1.endTheta=atan(-1/k); arc2.radis = obj.r; arc2.point = (to+from)/2 + 2*r * sin(alpha)/ sqrt(1+k*k)*[1, k]; arc2.startTheta=atan(-1/k)+pi -alpha + pi; arc2.endTheta=atan(-1/k)+2*pi + alpha + pi; arc3.radis = obj.r; arc3.point = to + (to-from)*obj.r/obj.omega; arc3.startTheta=atan(-1/k) + pi; arc3.endTheta=atan(-1/k)-alpha + pi; else arc1.radis = r; arc1.point = from - (to-from)*r/omega; arc1.startTheta=atan(-1/k)+pi+alpha + pi - alpha; arc1.endTheta=atan(-1/k)+pi + pi - alpha; arc2.radis = obj.r; arc2.point = (to+from)/2 + 2*r * sin(alpha)/ sqrt(1+k*k)*[1, k]; arc2.startTheta=atan(-1/k)+pi-alpha; arc2.endTheta=atan(-1/k)+2*pi+alpha; arc3.radis = obj.r; arc3.point = to + (to-from)*obj.r/obj.omega; arc3.startTheta=atan(-1/k) + alpha + pi; arc3.endTheta=atan(-1/k)-alpha + alpha + pi; end y = {y, Arc(arc1), Arc(arc2), Arc(arc3)}; end end