Matlab 与板凳龙和元宵灯

绘制单个灯笼
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
axes(Color='k',DataA=[1,1,1])
hold
a=315;
b=(0:.01:pi)';
[x,y,z]=sphere(a-1);
v=(abs(sin(20*b))+3)/4;
v(1:60)=nan;
A=ones(a).*v.*cat(3,1,0,0).*sin(b);
B=ones(a,a,3).*cat(3,1,1,0);
surf(x.*v,y.*v,z,A);
surf(x/2,y/2,z/2,B); 
shading flat
r=@rand;
plot3(r(9)*5-3,r(9)*6+4,r(9)*6+1,'w.');
view([0,-35])
camva(2)

单个灯笼

绘制灯笼群组
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
hold
a=315;
b=(0:.01:pi)';
[x,y,z]=sphere(a-1);
v=(abs(sin(20*b))+3)/4;
v(1:65)=nan;
A=ones(a).*v.^2.*cat(3,1,0,0).*sin(b).^2;
l=linspace(-1.6,2,15).^2;
for n=1:10
    s=2-n/8;
    surf(x.*v/s+n*2.8,y.*v/s,z/s+l(n)*3,A/s,EdgeA=0);
end
axis equal
view([78,-15]) 
set(gca,'color','k')
zlim([-60,63]);
camva(.25)

灯笼群组

板凳龙闹元宵

板凳龙”，又称“盘龙”，是浙闽地区的传统地方民俗文化活动。人们将少则几十条， 多则上百条的板凳首尾相连，形成蜿蜒曲折的板凳龙。盘龙时，龙头在前领头，龙身和龙尾相随盘旋，整体呈圆盘状。一般来说，在舞龙队能够自如地盘入和盘出的前提下，盘龙所需要的面积越小、行进速度越快，则观赏性越好。

某板凳龙由 223 节板凳组成，其中第 1 节为龙头，后面 221 节为龙身，最后 1 节为龙尾。龙头的板长为 341 cm，龙身和龙尾的板长均为 220 cm，所有板凳的板宽均为 30 cm。每节板凳上均有两个孔，孔径（孔的直径）为 5.5 cm，孔的中心距离最近的板头 27.5 cm。相邻两条板凳通过把手连接。

文件树
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
2024国赛A/
│   ├── get_boundy.m
│   ├── get_cor.m
│   ├── Q1_v.m
│   ├── Q2.m
│   ├── Q3/
│   │   ├── CheckCollison.m
│   │   ├── DrawSpiral.m
│   │   ├── GetBenXY.m
│   │   ├── GetFourPOint.m
│   │   ├── GetThetaArc.m
│   │   ├── get_boundy.m
│   │   ├── get_cor.m
│   │   ├── Q3_2.m

get_boundy.m
1
2
3
4
5
6
7
8
9
10
function [xb,yb] = get_boundy(x,y,r1,r2)
    x1 = x(1);
    x2 = x(2);
    y1 = y(1);
    y2 = y(2);
    syms x y
    [xb,yb] = solve((x - x1)*(x - x1) + (y - y1)*(y - y1) - r1*r1,(x - x2)*(x - x2) + (y - y2)*(y - y2) - r2*r2);
    xb = double(xb); 
    yb = double(yb);
end
get_cor.m
1
2
3
4
5
6
7
8
9
10
11
12
13
function f = get_cor(a,b,c,d,e)
% 已知前一个点坐标计算下一个点的坐标  
% a为螺线的A
% b为螺线的B
% c为前一个点的theata
% d为前一个点的rho
% e为半径
% f为所求点的rho
    syms x
    assume(x >= c - pi & x <= c + pi);
    f = solve( (b*x + a)^2 - 2*(b*x + a)*d*cos(x-c) + d^2 - e^2,x);
    f = double(f);
end
Q1_v.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
clc,clear,close all
warning off
mycolor1 = '#0b5e95';
mycolor2 = '#4664a5';
mycolor3 = '#7168b0';
mycolor4 = '#986bb5';
mycolor5 = '#bd6db3';
mycolor6 = '#de70ac';
mycolor7 = '#df9f1f';
mycolor8 = '#897456';
%% 求螺线
a = 0;
b = 0.55/2/pi;
theta = 0:0.05*pi:32*pi;
coe = b*theta;
x = coe.*cos(theta);
y = coe.*sin(theta);
plot(x,y,'--','Color',mycolor1);
axis([-9,9,-9,9])
axis equal
grid 
hold on 
%% 确定龙头位置
b = 0.55/2/pi;
s = 32*pi;
A = 0.275/pi;
B = s*sqrt(1+s^2) + log(s + sqrt(1 + s^2));
% 存储所有时刻各点的直角坐标和速度
AX = zeros(224,300);
AY = zeros(224,300);
AV = zeros(224,300);
k = 0;
for j = 0:300
    k = k + 1
    % 求第j秒时刻的龙头位置
    syms x1
    % 已知弧长求龙头坐标
    Headtheta = double(solve(0.5*A*(B - x1*sqrt(1 + x1^2) - log(x1 + sqrt(1 + x1^2))) == j,x1));
    Headrho = b*double(Headtheta);
    % 化为直角坐标
    HeadX = Headrho*cos(double(Headtheta));
    HeadY = Headrho*sin(double(Headtheta));
    plot(HeadX,HeadY,'^','Color',mycolor4,'LineWidth',1.75)
    % 绘制以龙头为圆心,以两孔间的距离为半径的圆
    CirRH = 2.86;  %龙头板凳长为半径
    CirRB = 1.65;  %龙身板凳长为半径
    CirT = linspace(0,2*pi,100);
    Cirx = HeadX + CirRH * cos(CirT);
    Ciry = HeadY + CirRH * sin(CirT);
    % plot(Cirx,Ciry,'r')
    %% 计算其他把手位置坐标
    % 开辟空间存放数据
    FlagList = ones(1,223);
    ThetList = ones(1,223);
    RhoList  = ones(1,223);
    XList    = ones(1,223);
    YList    = ones(1,223);
    VList    = ones(1,223);
    
    for i = 1:223
        %分两种情况计算 1.龙头后第一把手；2.其他把手的11
        if i == 1
            % 根据龙头位置确定下一个把手位置
            ThetList(i) = double(get_cor(a,b,Headtheta,Headrho,CirRH));
            RhoList(i)  = b*ThetList(i);
            if ThetList(i) > 32*pi
                FlagList(i) = 1;  % FlagList = 1表示未进入螺线   FlagList = 0表示未进入螺线
                XList(i) = 8.8;
                YList(i) = HeadY + sqrt(2.86*2.86 - (XList(i) - 8.8)^2);
            else
                FlagList(i) = 0;
                XList(i) = RhoList(i)*cos(ThetList(i));
                YList(i) = RhoList(i)*sin(ThetList(i));
            end
        else % 根据上一把手位置确定下一个把手位置
            ThetList(i) = double(get_cor(a,b,ThetList(i-1),RhoList(i-1),CirRB));
            RhoList(i)  = b*ThetList(i);
            if ThetList(i) > 32*pi
                FlagList(i) = 1;
                XList(i) = 8.8;
                YList(i) = YList(i-1) + sqrt(1.65*1.65 - (XList(i) - 8.8)^2);
            else
                FlagList(i) = 0;
                XList(i) = RhoList(i)*cos(ThetList(i));
                YList(i) = RhoList(i)*sin(ThetList(i));
            end
        end
    end
    %% 计算各把手速度
    HeadV = 1;
    Omega = HeadV/Headrho;
    ThetList = [Headtheta,ThetList];
    RhoList  = [Headrho,RhoList];
    XList = [HeadX,XList];
    YList = [HeadY,YList];
    VList = [HeadV,VList];
    FlagList = [0,FlagList];
    for i = 1:223
        if FlagList(i + 1) == 0  
            the1 = ThetList(i+1);
            the2 = ThetList(i);
            tempup = the1*cos(the1 - the2) - the2 + the1*the2*sin(the1 - the2);
            tempdown = the1-the2*cos(the1 - the2) + the1*the2*sin(the1 - the2);
            tempall  = tempup/tempdown * sqrt((1 + the1^2)/(1 + the2^2));
            VList(i+1) = abs(tempall) * VList(i);
        else
            VList(i+1) = VList(i);
        end
    end
    %% 画出把手和龙头位置
    plot(XList,YList,'-','Color',mycolor6)
    plot(XList(2:end),YList(2:end),'*','Color',mycolor6,'LineWidth',1.25)
    legend('螺线方程','龙头把手位置','龙身把手位置')
    AX(:,k) = XList';
    AY(:,k) = YList';
    AV(:,k) = VList';
end
Q2.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
clc,clear,close all
warning off
format long
% 使用二分法求解碰撞时间  大约 412.473837677 秒
%% 求螺线
a = 0
b = 0.55/2/pi;
theta = 0:0.05*pi:32*pi;
coe = b*theta;
x = coe.*cos(theta);
y = coe.*sin(theta);
plot(x,y,'--');
axis([-10,10,-10,10])
axis equal
grid 
hold on 
%% 确定龙头位置
s = 32*pi;
A = b;
B = s*sqrt(1+s^2) + log(s + sqrt(1 + s^2));
NumBody = 10;
k = 0;
CollisionTimeBegin = 300;
CollisionTimeEnd   = 420;
CollisionError = 0.2999999999999;
TimeError = CollisionTimeEnd - CollisionTimeBegin;
while TimeError > 0.00000001
j = (CollisionTimeEnd + CollisionTimeBegin)/2;

    k = k + 1;
    % 求第j秒时刻的龙头位置
    syms x1
    % 已知弧长求龙头坐标
    Headtheta = double(solve(0.5*A*(B - x1*sqrt(1 + x1^2) - log(x1 + sqrt(1 + x1^2))) == j,x1));
    Headrho = b*double(Headtheta);
    % 化为直角坐标
    HeadX = Headrho*cos(double(Headtheta));
    HeadY = Headrho*sin(double(Headtheta));
    % plot(HeadX,HeadY,'^r')
    % 绘制以龙头为圆心,以两孔间的距离为半径的圆
    CirRH = 2.86;  %龙头板凳长为半径
    CirRB = 1.65;  %龙身板凳长为半径
    CirT = linspace(0,2*pi,100);
    Cirx = HeadX + CirRH * cos(CirT);
    Ciry = HeadY + CirRH * sin(CirT);
    %% 计算其他把手位置坐标
    % 开辟空间存放数据
    FlagList = ones(1,NumBody);
    ThetList = ones(1,NumBody);
    RhoList  = ones(1,NumBody);
    XList    = ones(1,NumBody);
    YList    = ones(1,NumBody);
    VList    = ones(1,NumBody);
    for i = 1:NumBody
        %分两种情况计算 1.龙头后第一把手；2.其他把手的
        if i == 1
            % 根据龙头位置确定下一个把手位置
            ThetList(i) = double(get_cor(a,b,Headtheta,Headrho,CirRH));
            RhoList(i)  = b*ThetList(i);
            if ThetList(i) > 32*pi
                FlagList(i) = 1;  % FlagList = 1表示未进入螺线   FlagList = 0表示未进入螺线
                XList(i) = 8.8;
                YList(i) = HeadY + sqrt(2.86*2.86 - (XList(i) - 8.8)^2);
            else
                FlagList(i) = 0;
                XList(i) = RhoList(i)*cos(ThetList(i));
                YList(i) = RhoList(i)*sin(ThetList(i));
            end
        else % 根据上一把手位置确定下一个把手位置
            ThetList(i) = double(get_cor(a,b,ThetList(i-1),RhoList(i-1),CirRB));
            RhoList(i)  = b*ThetList(i);
            if ThetList(i) > 32*pi
                FlagList(i) = 1;
                XList(i) = 8.8;
                YList(i) = YList(i-1) + sqrt(1.65*1.65 - (XList(i) - 8.8)^2);
            else
                FlagList(i) = 0;
                XList(i) = RhoList(i)*cos(ThetList(i));
                YList(i) = RhoList(i)*sin(ThetList(i));
            end
        end
    end
    ThetList = [Headtheta,ThetList];
    RhoList  = [Headrho,RhoList];
    XList = [HeadX,XList];
    YList = [HeadY,YList];
    %% 计算端点
    BounListXF = []; % 板前面两个点x值
    BounListXB = []; % 板后面两个点x值
    BounListYF = []; % 板前面两个点y值
    BounListYB = []; % 板后面两个点y值
    for i = 1:NumBody
        if i == 1
            r1 = sqrt(0.275^2 + 0.15^2);
            r2 = sqrt((3.41-0.275)^2 + 0.15^2 );
            [xb1,yb1] = get_boundy (XList(i:i+1),YList(i:i+1),r1,r2);
            BounListXF = [BounListXF;xb1'];
            BounListYF = [BounListYF;yb1'];
            [xb2,yb2] = get_boundy (XList(i:i+1),YList(i:i+1),r2,r1);
            BounListXB = [BounListXB;xb2'];
            BounListYB = [BounListYB;yb2'];
            xx = [xb1',xb2(2),xb2(1),xb1(1)];
            yy = [yb1',yb2(2),yb2(1),yb1(1)];
            % fill(xx,yy,'r')
            % plot(xx,yy,'r-.')
        else
            r1 = sqrt(0.275^2 + 0.15^2);
            r2 = sqrt((2.2-0.275)^2 + 0.15^2 );
            [xb1,yb1] = get_boundy (XList(i:i+1),YList(i:i+1),r1,r2);
            BounListXF = [BounListXF;xb1'];
            BounListYF = [BounListYF;yb1'];
            [xb2,yb2] = get_boundy (XList(i:i+1),YList(i:i+1),r2,r1);
            BounListXB = [BounListXB;xb2'];
            BounListYB = [BounListYB;yb2'];
            xx = [xb1',xb2(2),xb2(1),xb1(1)];
            yy = [yb1',yb2(2),yb2(1),yb1(1)];
            %     % fill(xx,yy,'c')
            % plot(xx,yy,'r-.')
        end
    end
    %% 计算龙头左端点到各个点的距离
    % 判断龙头左端点
    x1 = BounListXF(1,1);
    y1 = BounListYF(1,1);
    x2 = BounListXF(1,2);
    y2 = BounListYF(1,2);
    dist1 = x1*x1 + y1*y1;
    dist2 = x2*x1 + y2*y2;
    if dist1>dist2
        HeadLeftX = x1;
        HeadLeftY = y1;
    else
        HeadLeftX = x2;
        HeadLeftY = y2;
    end
    % plot(HeadLeftX,HeadLeftY,'*')
    % 构造龙身内侧方程
    for i = 2:NumBody
        % 前端点
        x1 = BounListXF(i,1);
        y1 = BounListYF(i,1);
        x2 = BounListXF(i,2);
        y2 = BounListYF(i,2);
        dist1 = x1*x1 + y1*y1;
        dist2 = x2*x1 + y2*y2;
        if dist1<dist2
            BodyX1 = x1;
            BodyY1 = y1;
            BodyX3 = x2;
            BodyY3 = y2;
        else
            BodyX1 = x2;
            BodyY1 = y2;
            BodyX3 = x1;
            BodyY3 = y1;
        end
        % 后端点
        x1 = BounListXB(i,1);
        y1 = BounListYB(i,1);
        x2 = BounListXB(i,2);
        y2 = BounListYB(i,2);
        dist1 = x1*x1 + y1*y1;
        dist2 = x2*x1 + y2*y2;
        if dist1<dist2
            BodyX2 = x1;
            BodyY2 = y1;
            BodyX4 = x2;
            BodyY4 = y2;
        else
            BodyX2 = x2;
            BodyY2 = y2;
            BodyX4 = x1;
            BodyY4 = y1;
        end
        LineX1 = [BodyX1,BodyX2];
        LineY1 = [BodyY1,BodyY2];
        coef = polyfit(LineX1, LineY1, 1);
        LineK1=coef(1);
        LineB1=coef(2);
        xx = min(LineX1):0.0001:max(LineX1);
        yy = LineK1*xx + LineB1;
        % plot(xx,yy,'*')
        DisHL1(i) = abs(LineK1 * HeadLeftX - HeadLeftY + LineB1)/sqrt(LineK1^2 + 1);
        LineX2 = [BodyX3,BodyX4];
        LineY2 = [BodyY3,BodyY4];
        coef = polyfit(LineX2, LineY2, 1);
        LineK2=coef(1);
        LineB2=coef(2);
        xx = min(LineX2):0.0001:max(LineX2);
        yy = LineK2*xx + LineB2;
        DisHL2(i) = abs(LineK2 * HeadLeftX - HeadLeftY + LineB2)/sqrt(LineK2^2 + 1);
        
    end
    error = min(abs(DisHL1(2:end)-DisHL2(2:end)));
    fprintf('第 %.9f 秒龙头距离第二圈板凳两边距离的差为 %.9f\n', j,error);
    % error = 0.299938839970074;
    % CollisionError = 0.29999990000000000;
    if error < CollisionError
        CollisionTimeBegin = CollisionTimeBegin;
        CollisionTimeEnd   = j;
        fprintf('第 %.9f 秒撞了 \n', j);
    else
        CollisionTimeEnd = CollisionTimeEnd;
        CollisionTimeBegin = j;
        fprintf('第 %.9f 秒没撞\n', j);
    end
    TimeError = CollisionTimeEnd - CollisionTimeBegin;

end
CheckCollison.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
function Error = CheckCollsion(ForwardPoint,BackPoint,CheckPoint)
    HeadLeftX = CheckPoint(1);
    HeadLeftY = CheckPoint(2);
    x1 = ForwardPoint(1,1);
    y1 = ForwardPoint(1,2);
    x2 = ForwardPoint(2,1);
    y2 = ForwardPoint(2,2);
    dist1 = x1*x1 + y1*y1;
    dist2 = x2*x1 + y2*y2;
    if dist1<dist2
        BodyX1 = x1;
        BodyY1 = y1;
        BodyX3 = x2;
        BodyY3 = y2;
    else
        BodyX1 = x2;
        BodyY1 = y2;
        BodyX3 = x1;
        BodyY3 = y1;
    end
    % 后端点
    x1 = BackPoint(1,1);
    y1 = BackPoint(1,2);
    x2 = BackPoint(2,1);
    y2 = BackPoint(2,2);
    dist1 = x1*x1 + y1*y1;
    dist2 = x2*x1 + y2*y2;
    if dist1<dist2
        BodyX2 = x1;
        BodyY2 = y1;
        BodyX4 = x2;
        BodyY4 = y2;
    else
        BodyX2 = x2;
        BodyY2 = y2;
        BodyX4 = x1;
        BodyY4 = y1;
    end
    LineX1 = [BodyX1,BodyX2];
    LineY1 = [BodyY1,BodyY2];
    coef = polyfit(LineX1, LineY1, 1);
    LineK1=coef(1);
    LineB1=coef(2);
    xx = min(LineX1):0.0001:max(LineX1);
    yy = LineK1*xx + LineB1;
    % plot(xx,yy,'*')
    Distance1 = abs(LineK1 * HeadLeftX - HeadLeftY + LineB1)/sqrt(LineK1^2 + 1)
    LineX2 = [BodyX3,BodyX4];
    LineY2 = [BodyY3,BodyY4];
    coef = polyfit(LineX2, LineY2, 1);
    LineK2=coef(1);
    LineB2=coef(2);
    xx = min(LineX2):0.0001:max(LineX2);
    yy = LineK2*xx + LineB2;
    Distance2= abs(LineK2 * HeadLeftX - HeadLeftY + LineB2)/sqrt(LineK2^2 + 1);
    Error = abs(Distance1 - Distance2);
end
DrawSpiral.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
function DrawSpiral(a,theta1,theta2)
    theta = theta1:0.005*pi:theta2;%θ的范围和步长，同时也可以控制螺线的旋转方向
    coe = a*theta;%阿基米德螺线方程
    x = coe.*cos(theta);%因使用需要，获取直角坐标系下x轴的坐标并进行四舍五入
    y = coe.*sin(theta);%因使用需要，获取直角坐标系下y轴的坐标并进行四舍五入
    plot(x,y,'--');%将获取的坐标打印在图纸上
    axis([-6,6,-6,6])
    axis equal
    grid on
    hold on
    x = 4.5*cos(theta);
    y = 4.5*sin(theta);
    plot(x,y,'b')
end
get_boundy.m
1
2
3
4
5
6
7
8
9
10
function [xb,yb] = get_boundy(xx,yy,r1,r2)
    x1 = xx(1);
    x2 = xx(2);
    y1 = yy(1);
    y2 = yy(2);
    syms x y
    [xb,yb] = solve((x - x1)*(x - x1) + (y - y1)*(y - y1) - r1*r1,(x - x2)*(x - x2) + (y - y2)*(y - y2) - r2*r2);
    xb = double(xb); 
    yb = double(yb);
end
get_cor.m
1
2
3
4
5
6
7
8
9
10
11
12
13
function f = get_cor(a,b,c,d,e)
% 已知前一个点坐标计算下一个点的坐标  
% a为螺线的A
% b为螺线的B
% c为前一个点的theata
% d为前一个点的rho
% e为半径
% f为所求点的rho
    syms x
    assume(x >= c - pi & x <= c + pi);
    f = solve( (b*x + a)^2 - 2*(b*x + a)*d*cos(x-c) + d^2 - e^2,x);
    f = double(f);
end
GetBenXY.m
1
2
3
4
5
6
7
8
9
10
11
12
function [BenTheta,BenRho,BenX,BenY] = GetBenXY(a,b,theta,rho,r)
    % a 螺线的常数
    % b 螺线的参数
    % theta 上一个点的theta
    % rho 上一个点的rho
    % BenTheta 输出当前的theta
    % BneRho 输出当前的Rho
    BenTheta = double(get_cor(a,b,theta,rho,r));
    BenRho   = b*BenTheta;
    BenX     = BenRho * cos(BenTheta);
    BenY     = BenRho * sin(BenTheta);
end
GetFourPOint.m
1
2
3
4
5
6
7
8
9
10
11
function [xx,yy] = GetFourPOint(x1,x2,y1,y2,r1,r2)
    % x1,y1，x2,y2 把手的坐标
    % r1,r2 通过两圆相交求端点，此为两圆的半径
    % xx,yy 四个端点的坐标
    [xb1,yb1] = get_boundy ([x1,x2],[y1,y2],r1,r2);
    [xb2,yb2] = get_boundy ([x1,x2],[y1,y2],r2,r1);
    xx = [xb1',xb2(2),xb2(1),xb1(1)];
    yy = [yb1',yb2(2),yb2(1),yb1(1)];
    fill(xx,yy,'r')
    plot(xx,yy,'r-.')
end
GetThetaArc.m
1
2
3
4
5
6
7
8
9
10
11
function  d = GetThetaArc(A,s,c)
% a 螺线系数
% b 开始时刻
% c 弧长
% d 输出theta
    B = s*sqrt(1+s^2) + log(s + sqrt(1 + s^2));
    % 求第j秒时刻的龙头位置
    syms x1
    % 已知弧长求龙头坐标
    d = double(solve(0.5*A*(B - x1*sqrt(1 + x1^2) - log(x1 + sqrt(1 + x1^2))) == c,x1));
end
Q3_2.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
clc,clear,close all
warning off
format long
% 使用二分法求最小螺距
% 判断标准为 设在龙头到达掉头区域边界的时间为t,计算前[t-20,t]秒内是有碰撞
% 然后使用二分判断下一个，知道二分区间小于0.000001为止
% 最优逻辑大约为0.450336932341416
%% 求螺线
%% 确定龙头位置
BBegin = 0.30;
BEnd   = 0.55;
BError = BEnd - BBegin;
NumBenches = 30;
LengthBody = 1.65;
LengthHead = 2.86;
dt = 0.1;
a = 0;
CollisionError = 0.2999999;
CheckList = [];
ErrorList = [];
while BError > 0.000001
    BMid = (BBegin + BEnd)/2
    b = BMid/2/pi;
    Theta2 = 32*pi; % 不妨假设龙头出发点还是在32\pi位置，但此时已不是A点
    Theta1 = 4.5/b;
    ArcFun = @(x) b*x;
    LenArc = integral(ArcFun,Theta1,Theta2);


    %% 计算时间端点
    TimeStart = LenArc-20;
    TimeEnd   = LenArc;
    k = 0;
    CheckFlag = zeros(1,(TimeEnd-TimeStart)/dt+1);
    error = zeros(1,(TimeEnd-TimeStart)/dt+1);
    for j = TimeEnd:-dt:TimeStart
        figure(1)
        hold off
        DrawSpiral(b,Theta1,Theta2);
        k = k + 1; 
        %% 计算各把手位置
        HeadTheta = GetThetaArc(b,Theta2,j);
        HeadRho = b*double(HeadTheta);
        HeadX = HeadRho*cos(double(HeadTheta));
        HeadY = HeadRho*sin(double(HeadTheta));
        L = ones(1,NumBenches)*LengthBody;
        FlagList = ones(1,NumBenches+1);
        ThetList = ones(1,NumBenches+1);
        RhoList  = ones(1,NumBenches+1);
        XList    = ones(1,NumBenches+1);
        YList    = ones(1,NumBenches+1);
        L(1) = LengthHead;
        XList(1) = HeadX;
        YList(1) = HeadY;
        ThetList(1) = HeadTheta;
        RhoList(1)  = HeadRho; 

        for i = 2:NumBenches + 1
            [BenTheta,BenRho,BenX,BenY] = GetBenXY(a,b,ThetList(i-1),RhoList(i-1),L(i-1));
            ThetList(i) = BenTheta;
            RhoList(i)  = BenRho;
            XList(i)    = BenX;
            YList(i)    = BenY;
        end
        plot(XList,YList,'-*r')
        %% 计算端点
        BounListXF = []; % 板前面两个点x值
        BounListXB = []; % 板后面两个点x值
        BounListYF = []; % 板前面两个点y值
        BounListYB = []; % 板后面两个点y值
        for i = 1:NumBenches
            if i == 1
                r1 = sqrt(0.275^2 + 0.15^2);
                r2 = sqrt((3.41-0.275)^2 + 0.15^2 );
                [xb1,yb1] = get_boundy (XList(i:i+1),YList(i:i+1),r1,r2);
                BounListXF = [BounListXF;xb1'];
                BounListYF = [BounListYF;yb1'];
                [xb2,yb2] = get_boundy (XList(i:i+1),YList(i:i+1),r2,r1);
                BounListXB = [BounListXB;xb2'];
                BounListYB = [BounListYB;yb2'];
                xx = [xb1',xb2(2),xb2(1),xb1(1)];
                yy = [yb1',yb2(2),yb2(1),yb1(1)];
                fill(xx,yy,'r')
                plot(xx,yy,'r-.')
            else
                r1 = sqrt(0.275^2 + 0.15^2);
                r2 = sqrt((2.2-0.275)^2 + 0.15^2 );
                [xb1,yb1] = get_boundy (XList(i:i+1),YList(i:i+1),r1,r2);
                BounListXF = [BounListXF;xb1'];
                BounListYF = [BounListYF;yb1'];
                [xb2,yb2] = get_boundy (XList(i:i+1),YList(i:i+1),r2,r1);
                BounListXB = [BounListXB;xb2'];
                BounListYB = [BounListYB;yb2'];
                xx = [xb1',xb2(2),xb2(1),xb1(1)];
                yy = [yb1',yb2(2),yb2(1),yb1(1)];
                fill(xx,yy,'c')
                plot(xx,yy,'r-.')
            end
        end
        %% 计算龙头端点到各个点的距离
        % 判断龙头左前端点
        x1 = BounListXF(1,1);
        y1 = BounListYF(1,1);
        x2 = BounListXF(1,2);
        y2 = BounListYF(1,2);
        dist1 = x1*x1 + y1*y1;
        dist2 = x2*x1 + y2*y2;
        if dist1>dist2
            HeadLeftX = x1;
            HeadLeftY = y1;
        else
            HeadLeftX = x2;
            HeadLeftY = y2;
        end
        % plot(HeadLeftX,HeadLeftY,'*')
        % 构造龙身内侧方程
        CheckPoint = [HeadLeftX,HeadLeftY];
        for i = 2:NumBenches
            % 前端点
            ForwardPoint = [BounListXF(i,1),BounListYF(i,1);
                            BounListXF(i,2),BounListYF(i,2)];
            BackPoint =    [BounListXB(i,1),BounListYB(i,1);
                            BounListXB(i,2),BounListYB(i,2)];
            DistForward(i) = CheckCollison(ForwardPoint,BackPoint,CheckPoint);
        end
        error(k) = min(DistForward(2:end));
        fprintf('第 %.9f 秒龙头左前距离第二圈板凳两边距离的差为 %.9f\n', j,error(k));
        if error(k) < CollisionError
            CheckFlag = 1;
            fprintf(' %0.9f 螺距 %0.9f 有碰撞\n', BMid, j);
            break;
        else
            CheckFlag(k) = 0;
            fprintf(' %0.9f 螺距 %0.9f 没问题\n', BMid, j);
        end
        % 判断龙头左后端点
        x1 = BounListXB(1,1);
        y1 = BounListYB(1,1);
        x2 = BounListXB(1,2);
        y2 = BounListYB(1,2);
        dist1 = x1*x1 + y1*y1;
        dist2 = x2*x1 + y2*y2;
        if dist1>dist2
            HeadLeftX = x1;
            HeadLeftY = y1;
        else
            HeadLeftX = x2;
            HeadLeftY = y2;
        end
        % plot(HeadLeftX,HeadLeftY,'*')
        % 构造龙身内侧方程
        CheckPoint = [HeadLeftX,HeadLeftY];
        for i = 2:NumBenches
            % 前端点
            ForwardPoint = [BounListXF(i,1),BounListYF(i,1);
                            BounListXF(i,2),BounListYF(i,2)];
            BackPoint =    [BounListXB(i,1),BounListYB(i,1);
                            BounListXB(i,2),BounListYB(i,2)];
            DistBack(i) = CheckCollison(ForwardPoint,BackPoint,CheckPoint);
        end
        error(k) = min(DistBack(2:end));
        fprintf('第 %.9f 秒龙头左后端点距离第二圈板凳两边距离的差为 %.9f\n', j,error(k));
        if error(k) < CollisionError
            CheckFlag = 1;
            fprintf(' %0.9f 螺距 %0.9f 有碰撞\n', BMid, j);
            break;
        else
            CheckFlag(k) = 0;
            fprintf(' %0.9f 螺距 %0.9f 没问题\n', BMid, j);
        end

    end % j循环结束
    CheckList = [CheckFlag;CheckFlag];
    ErrorList = [ErrorList;error];
    if max(CheckFlag) == 1 %等于1 说明有碰撞 扩大螺距
        BBegin = BMid;
        BEnd   = BEnd;
    else %等于0 说明没有碰撞 缩小螺距
        BBegin = BBegin;
        BEnd   = BMid;
    end
    BError = BEnd - BBegin;
end % while 循环结束