-
Notifications
You must be signed in to change notification settings - Fork 5
/
SSri_RandomInitialization.m
142 lines (101 loc) · 4.35 KB
/
SSri_RandomInitialization.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
%--------------------------AE-AE5810 Space Thesis--------------------------
%
%
% Function : Random Initial Positioning
% Programing Language : MatLab R2018a
% Project : MSc. Thesis
% Copyright : Andrés Ripoll Sánchez
% Year : 2019
%
%--------------------------------------------------------------------------
%
% Synopsis : This algorithm generates the initial positions
% of the agents. To do so it randomly poses each agent around
% one of the grid points under two assupmtions. One agent per
% grid point and all agents connected.
%
% Inputs SSis_arAgents: Array with all the agents of the swarm.
%
% Output
%
%--------------------------------------------------------------------------
function SSri_RandomInitialization2(SSis_arAgents)
% Load Global Variables Used
global SSis_rKnowledgeRadius SSis_rMovementRadius SSis_iNumAgents
% Initialize Variables
iLoop = 0;
% Select the Number of Variables to be Created
if all(SSis_arAgents(1).oState.csType == 'Squa' )|| ...
all( SSis_arAgents(1).oState.csType == 'Hexa' )
iNumVar = 2;
else
iNumVar = 3;
end
% Loop over all agents
for iLoop = 1:SSis_iNumAgents
if iLoop == 1
SSis_arAgents(iLoop).Position = [0 0 0]';
SSis_arAgents(iLoop).Velocity = zeros(3,1);
iCurrentInitialized = 1;
continue
end
while 1>0
% Select an Agent
iAgentSelected = randi(iCurrentInitialized);
% Evaluate State
SSis_arAgents(iAgentSelected).oState.Create(SSis_arAgents( ...
1:iCurrentInitialized),iAgentSelected)
% Check if the state is full
arTotalAgents = size( SSis_arAgents(iAgentSelected).oState. ...
arState,1) * size( SSis_arAgents(iAgentSelected). ...
oState.arState,2 );
if sum(sum(SSis_arAgents(iAgentSelected).oState. ...
arState,2), 1) < arTotalAgents
break
end
end
% Create Dummy state
arDummyState = ones( size(SSis_arAgents(iAgentSelected). ...
oState.arState) );
% Create State with Empty Positions
arDummyState = arDummyState - SSis_arAgents(iAgentSelected). ...
oState.arState;
% Randomly Pick One
lFound = false;
while ~lFound
pos1 = randi(size( SSis_arAgents(iAgentSelected).oState. ...
arState,1));
pos2 = randi(size( SSis_arAgents(iAgentSelected).oState. ...
arState,2));
if arDummyState(pos1,pos2) == 1
lFound = true;
end
end
% Create State with only the Possition Selected Position
arDummyState = zeros( size(SSis_arAgents(iAgentSelected). ...
oState.arState) );
arDummyState(pos1,pos2) = 1;
% Set Reference Point to Grid
arDummyPoint = SSis_arAgents(iAgentSelected).gridpoint( ...
SSis_arAgents(iAgentSelected).Position);
% Translate to Cartesian
SSis_arAgents(iLoop).Position = SSis_arAgents(iLoop).oState. ...
TranslateState(arDummyState)' + arDummyPoint;
SSis_arAgents(iLoop).Velocity = zeros(3,1);
iCurrentInitialized = iCurrentInitialized + 1;
end
% Add Erros
for iLoop2 = 1:SSis_iNumAgents
% Create an error within the interval [-SSis_rMovementRadius/4
% SSis_rMovementRadius/4]
arError = (rand(iNumVar,1) * (SSis_rMovementRadius) - ...
SSis_rMovementRadius/2)/4;
if iNumVar == 2
SSis_arAgents(iLoop2).Position = SSis_arAgents(iLoop2). ...
Position + [0 arError(1) arError(2)]';
else
SSis_arAgents(iLoop2).Position = SSis_arAgents(iLoop2). ...
Position + arError;
end
end
end