FormationControlSimulation/SOURCE/rigidityMatrixFnc.m

25 lines
997 B
Matlab

function [R,K] = rigidityMatrixFnc (edgeL)
%% Fungsi ini digunakan untuk menggenerasi fungsi rigidity R.
%% diaman fungsi tersebut adalah hasil turunan dari fungsi edge
addpath("./networks-toolbox");
pkg load symbolic
% generasi koneksi bentuk incidence
tmp = edgeL2adj(edgeL);
gInc = adj2inc(tmp);
%% drawCircGraph(tmp)
tmp = getNodes(gInc,'adjacency')
% generasi vector error menggunakan matrix
% koneksi
K = kron(adj2inc(edgeL2adj(edgeL))',eye(2));
edgeVector = @(x,k) k*x;
% hasil turunan dari fungsi edge
str = "errBlockDiagonal = @(errVec) (blkdiag(";
for i = 1:2:(length(tmp)*2)-2
str = strcat(str,sprintf("errVec(%i:%i)', ",i,i+1));
endfor
str = strcat(str,sprintf("errVec(%i:%i)' ));",i+2,i+3));
% return sebagai fungsi R
eval(str)
R = @(x,k) errBlockDiagonal(edgeVector(x,k))*k;
endfunction