-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathevaluate_matrix.m
65 lines (44 loc) · 1.98 KB
/
evaluate_matrix.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
function [dG_H,G_H] = evaluate_matrix(x,H,pd,geo)
% Inputs: x, H, pd, and geo
% the evaluation points have coordinates (x(i),H)
L = pd.L;
geo_large = geometry;
geo_large = geo_large.cctor(3,2);
geo_large.obstacles(1) = geo.obstacles(1);
geo_large.obstacles(2) = geo.obstacles(1);
geo_large.obstacles(3) = geo.obstacles(1);
geo_large.segments(1) = geo.segments(1);
geo_large.segments(2) = geo.segments(2);
geo_large.obstacles(1).x(:,1) = geo.obstacles(1).x(:,1)-L;
geo_large.obstacles(3).x(:,1) = geo.obstacles(1).x(:,1)+L;
geo_large.segments(1).x(:,1) = geo.segments(1).x(:,1)-L;
geo_large.segments(1).x_start(:,1) = geo.segments(:,1).x_start(1)-L;
geo_large.segments(1).x_end(:,1) = geo.segments(:,1).x_end(1)-L;
geo_large.segments(2).x(:,1) = geo.segments(2).x(:,1)+L;
geo_large.segments(2).x_start(:,1) = geo.segments(2).x_start(:,1)+L;
geo_large.segments(2).x_end(:,1) = geo.segments(2).x_end(:,1)+L;
Nx = numel(x(:));
pts = [x' repmat(H,Nx,1)];
pot_ext = potentials;
grad_pot_ext = grad_potentials;
gm = pd.gm;
A0 = zeros(Nx,2*geo.obstacles.Np);
B0 = zeros(Nx,2*geo.obstacles.Np);
for o=1:3
pot_ext = pot_ext.cctor_local(pd.k(1),pts,geo_large.obstacles(o));
grad_pot_ext = grad_pot_ext.cctor_local(pd.k(1),pts,geo_large.obstacles(o));
A0 = A0+gm^(o-2)*[grad_pot_ext.DL(Nx+1:end,:) -grad_pot_ext.SL(Nx+1:end,:)];
B0 = B0+gm^(o-2)*[pot_ext.DL -pot_ext.SL];
end
pot_ext = pot_ext.cctor_local(pd.k(1),pts,geo_large.segments(1));
grad_pot_ext = grad_pot_ext.cctor_local(pd.k(1),pts,geo_large.segments(1));
W = diag(geo_large.segments(1).win);
A1 = gm^(-1)*[grad_pot_ext.DL(Nx+1:end,:)*W -grad_pot_ext.SL(Nx+1:end,:)*W];
B1 = gm^(-1)*[pot_ext.DL*W -pot_ext.SL*W];
pot_ext = pot_ext.cctor_local(pd.k(1),pts,geo_large.segments(2));
grad_pot_ext = grad_pot_ext.cctor_local(pd.k(1),pts,geo_large.segments(2));
A1 = A1 + gm^2*[-grad_pot_ext.DL(Nx+1:end,:)*W grad_pot_ext.SL(Nx+1:end,:)*W];
B1 = B1 + gm^2*[-pot_ext.DL*W pot_ext.SL*W];
dG_H = [A0 A1];
G_H = [B0 B1];
end