function [x y z] = cg3d(teta)
if (rows(teta)==1)
 teta = teta';
endif
alfa1 = 90;	a1 = 750	;	d1 = 590;
alfa2 = 0;	a2 = 1350	;	d2 = 0;
alfa3 = -90;	a3 = -41	;	d3 = 0;
alfa4 = 90;	a4 = 0		;	d4 = 1400;
%alfa5 = 90	;	a5 = 215	;	d5 = 0		;	teta5 = 0	;	min_teta5 = -125	;	max_teta5 = 125		;
%alfa6 = 0	;	a6 = 0		;	d6 = 240	;	teta6 = 0	;	min_teta6 = -350	;	max_teta6 = 350		;
teta1=teta(1);teta2=teta(2);teta3=teta(3);teta4=teta(4);

T01 = [	cosd(teta1)	-cosd(alfa1)*sind(teta1)	sind(teta1)*sind(alfa1)		a1*cosd(teta1)	;
	sind(teta1)	cosd(teta1)*cosd(alfa1)		-sind(alfa1)*cosd(teta1)	a1*sind(teta1)	;
	0		sind(alfa1)			cosd(alfa1)			d1		;
	0		0				0				1		];

T12 = [	cosd(teta2)	-cosd(alfa2)*sind(teta2)	sind(teta2)*sind(alfa2)		a2*cosd(teta2)	;
	sind(teta2)	cosd(teta2)*cosd(alfa2)		-sind(alfa2)*cosd(teta2)	a2*sind(teta2)	;
	0		sind(alfa2)			cosd(alfa2)			d2		;
	0		0				0				1		];

T23 = [	cosd(teta3)	-cosd(alfa3)*sind(teta3)	sind(teta3)*sind(alfa3)		a3*cosd(teta3)	;
	sind(teta3)	cosd(teta3)*cosd(alfa3)		-sind(alfa3)*cosd(teta3)	a3*sind(teta3)	;
	0		sind(alfa3)			cosd(alfa3)			d3		;
	0		0				0				1		];

T34 = [	cosd(teta4)	-cosd(alfa4)*sind(teta4)	sind(teta4)*sind(alfa4)		a4*cosd(teta4)	;
	sind(teta4)	cosd(teta4)*cosd(alfa4)		-sind(alfa4)*cosd(teta4)	a4*sind(teta4)	;
	0		sind(alfa4)			cosd(alfa4)			d4		;
	0		0				0				1		];

%				for teta5 = min_teta5 : step5 : max_teta5
%
%T45 = [	cosd(teta5)	-cosd(alfa5)*sind(teta5)	sind(teta5)*sind(alfa5)		a5*cosd(teta5)	;
%	sind(teta5)	cosd(teta5)*cosd(alfa5)		-sind(alfa5)*cosd(teta5)	a5*sind(teta5)	;
%	0		sind(alfa5)			cosd(alfa5)			d5		;
%	0		0				0				1		];
%
%					for teta6 = min_teta6 : step6 : max_teta6
%
%T56 = [	cosd(teta6)	-cosd(alfa6)*sind(teta6)	sind(teta6)*sind(alfa6)		a6*cosd(teta6)	;
%	sind(teta6)	cosd(teta6)*cosd(alfa6)		-sind(alfa6)*cosd(teta6)	a6*sind(teta6)	;
%	0		sind(alfa6)			cosd(alfa6)			d6		;
%	0		0				0				1		];

T02 = T01*T12;
T03 = T02*T23;
T04 = T03*T34;%*T45*T56

x(1,5) = T04( 1, 4 );
y(1,5) = T04( 2, 4 );
z(1,5) = T04( 3, 4 );
x(1,4) = T03( 1, 4 );
y(1,4) = T03( 2, 4 );
z(1,4) = T03( 3, 4 );
x(1,3) = T02( 1, 4 );
y(1,3) = T02( 2, 4 );
z(1,3) = T02( 3, 4 );
x(1,2) = T01( 1, 4 );
y(1,2) = T01( 2, 4 );
z(1,2) = T01( 3, 4 );
x(1,1)=0;y(1,1)=0;z(1,1)=0;

endfunction