% Portal

clear all

alpha4 = atan(4/3) ; 
alpha5 = atan(4/2) ; 
alpha6 = atan(5/1) ; 
alpha7 = atan(1/1) ;

c4 = cos(alpha4) ;
c5 = cos(alpha5) ;
c6 = cos(alpha6) ;
c7 = cos(alpha7) ;

s4 = sin(alpha4) ;
s5 = sin(alpha5) ;
s6 = sin(alpha6) ;
s7 = sin(alpha7) ;

%    23 24  25 26  27 28 29 30  31 32 33  34 35 36  37 38  39 1x 1y 2y
A = [ 0  0   0  0   0  0  0  0   0  0  0  c6 c5 c4  0   0   0  1  0  0 ;   %1x
      0  0   0  0   0  0  0  0   0  0  0  s6 s5 s4  0   0   0  0  1  0 ;   %1y
      0  0   0  0   0  0  0  0   0  0  0  0  0  0  -c4 -c5 -c6 0  0  0 ;   %2x
      0  0   0  0   0  0  0  0   0  0  0  0  0  0   s4  s5  s6 0  0  1 ;   %2y
      0  0   0  0   0  0  0  c7  0  1  0  0 -c5 0   0   0   0  0  0  0 ;   %3x
      0  0   0  0   0  0  0  s7  0  0  0  0 -s5 0   0   0   0  0  0  0 ;   %3y
      0  0   0  0   0  0  0  0   0 -1  1  0  0 -c4  c4  0   0  0  0  0 ;   %4x
      0  0   0  0   0  0  0  0   0  0  0  0  0 -s4 -s4  0   0  0  0  0 ;   %4y
      0  0   0  0   0  0  0  0  -c7 0 -1  0  0  0   0   c5  0  0  0  0 ;   %5x
      0  0   0  0   0  0  0  0   s7 0  0  0  0  0   0  -s5  0  0  0  0 ;   %5y
      0  c7  0  0   0  1  0  0   0  0  0 -c6 0  0   0   0   0  0  0  0 ;   %6x
      0  s7  0  0   0  0  0  0   0  0  0 -s6 0  0   0   0   0  0  0  0 ;   %6y
      0  0  -c7 c7  0 -1  1 -c7  c7 0  0  0  0  0   0   0   0  0  0  0 ;   %7x
      0  0   s7 s7  0  0  0 -s7 -s7 0  0  0  0  0   0   0   0  0  0  0 ;   %7y
      0  0   0  0  -c7 0 -1  0   0  0  0  0  0  0   0   0   c6 0  0  0 ;   %8x
      0  0   0  0   s7 0  0  0   0  0  0  0  0  0   0   0  -s6 0  0  0 ;   %8y
      1 -c7  c7 0   0  0  0  0   0  0  0  0  0  0   0   0   0  0  0  0 ;   %9x
      0 -s7 -s7 0   0  0  0  0   0  0  0  0  0  0   0   0   0  0  0  0 ;   %9y
     -1  0   0 -c7  c7 0  0  0   0  0  0  0  0  0   0   0   0  0  0  0 ;   %10x
      0  0   0 -s7 -s7 0  0  0   0  0  0  0  0  0   0   0   0  0  0  0 ] ; %10y
      
b = [0 ; 0 ; 0 ; 0 ;    0  ;   0  ; 0 ;   0   ;    0   ;   0 ; ...
     0 ; 0 ; 0 ; 0 ; 64/29 ; 5/29 ; 0 ; -9/29 ; -35/29 ; 33/29 ] ;

FSi = A \ b ;

for i=1:17
 disp(['FS', num2str(i+22) , ' = ' , num2str(FSi(i))]) ;
end
disp(['FP1x = ' , num2str(FSi(18))]) ;
disp(['FP1y = ' , num2str(FSi(19))]) ;
disp(['FP2y = ' , num2str(FSi(20))]) ;
