-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathRoomRecovery_4wall.m
126 lines (120 loc) · 5.71 KB
/
RoomRecovery_4wall.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
function RoomRecovery_4wall(G1,G2)
%input 收到的G1(1x4) G2(1x6)
%G1是第一世代聲源大小
%G2是第二世代聲源大小(成對出現) EX 在此因為Q_ij Q_ji距離原點一樣遠=>只會在G2出現一次
% RoomRecovery_4wall(G1,G2)
n_Q_pi=0;
for ii1=1:6
for ii2=1:6
if ii1==ii2
continue
end
for ii3=1:6
if (ii1==ii3) | (ii2==ii3)
continue
end
for ii4=1:6
if (ii1==ii4) | (ii2==ii4) | (ii3==ii4)
continue
end
for ii5=1:6
if (ii1==ii5 )|( ii2==ii5) |( ii3==ii5)|(ii4==ii5)
continue
end
for ii6=1:6
if (ii1==ii6) | (ii2==ii6) | (ii3==ii6)|(ii4==ii6)|(ii5==ii6)
continue
end
%disp([ii1,ii2,ii3,ii4,ii5,ii6])
%排列組合G2完成
%創造Q_pi
Q_pi=[0,G2(ii1),G2(ii2),G2(ii3);
G2(ii1),0,G2(ii4),G2(ii5);
G2(ii2),G2(ii4),0,G2(ii6);
G2(ii3),G2(ii5),G2(ii6),0] ;
if rank(Q_pi) >4
break
end
n_Q_pi=n_Q_pi+1;
%從這開始造A_pi
n_A_pi=0;
for i1=1:4
for i2=1:4
if i1==i2
continue
end
for i3=1:4
if (i3==i2)|(i3==i1)
continue
end
for i4=1:4
if (i4==i3)|(i4==i2)|(i4==i1)
continue
end
%A_pi造完了
% disp([i1,i2,i3,i4])
n_A_pi=n_A_pi+1;
A_pi=[G1(1),0,0,0;
0,G1(2),0,0;
0,0,G1(3),0;
0,0,0,G1(4)];
NTN=A_pi*A_pi*ones(4)+ones(4)*A_pi*A_pi-Q_pi;
if rank(NTN)==2
%成功找到適合的NTN
NTN=1/2* inv(A_pi)*NTN*inv(A_pi);
%SVD
[V,D] = eig(NTN);
NTN
iilamda2=0;
for iilamda=1:4
if D(iilamda,iilamda)~=0
iilamda2=iilamda2+1;
D2(iilamda2,iilamda2)=sqrt(D(iilamda,iilamda));
for iii=1:4
V2(iii,iilamda2)=V(iii,iilamda);
end
end
end
N=D2*transpose(V2)
A_pi
Q_pi
NTN_usingNewN=transpose(N)*N
for iiii=1:4
recoverX(iiii)=N(2,iiii);
recoverY(iiii)=-1*N(1,iiii);
recoverDistance(iiii)=(1/2)*A_pi(iiii,iiii);
recoverPointX(iiii)=N(1,iiii)*recoverDistance(iiii);
recoverPointY(iiii)=N(2,iiii)*recoverDistance(iiii);
PointX=2*N(1,iiii)*recoverDistance(iiii)
PointY=2*N(2,iiii)*recoverDistance(iiii)
plot(PointX,PointY,"r:diamond")
hold on
end
recoverX
recoverY
recoverPointX
recoverPointY
for iiii=1:4
t= linspace (-10,10);
X=recoverPointX(iiii)+t*recoverX(iiii);
Y=recoverPointY(iiii)+t*recoverY(iiii);
plot(X,Y)
hold on
end
plot(0,0,"k:diamond")
hold on
return;
end
end
end
end
end
n_A_pi;
end
end
end
end
end
end
n_Q_pi;
end