%% Plot 3d-Verschiebung

load('D:\USERDATA\FB2HO01\Forschung\Trajekorienplanung\Stützpunktberechnung\Rohpunkte_Bst1.3.mat')

figure 
hold on
axis equal

MRSt = transl(-95.7,0,190)*trotz(-90,'deg');
MTHr = transl(0,0,225.5)*trotz(90-3.75,'deg')*transl(-2.5,99.971,0)*troty(90,'deg');


pTrajRS_ohneTCP = MRSt*[cWindingRS.pWinding;zeros(1,size(cWindingRS.pWinding,2));ones(1,size(cWindingRS.pWinding,2))];
pTrajTH_ohneTCP = MTHr*[cWindingTH.pWinding;zeros(1,size(cWindingTH.pWinding,2));ones(1,size(cWindingTH.pWinding,2))];


r = 0.789/2;

for i = 1:1:size(pTrajRS_ohneTCP,2)
    circle3(pTrajRS_ohneTCP(1,i),pTrajRS_ohneTCP(2,i),pTrajRS_ohneTCP(3,i),[0;0;1],r,'black')
end


for i = 1:1:size(pTrajTH_ohneTCP,2)
    circle3(pTrajTH_ohneTCP(1,i),pTrajTH_ohneTCP(2,i),pTrajTH_ohneTCP(3,i),cross([pTrajTH_ohneTCP(1,i);pTrajTH_ohneTCP(2,i);pTrajTH_ohneTCP(3,i)],[0;0;1]),r,'black')
end

box off
tightfig;
axis off