function result = fx(thi,theta,t,t_0,v,h)


t1   = -inv([2*cos(thi)*cos(theta)-v   -2*cos(thi)*sin(theta)*(t-t_0)   -2*sin(thi)*cos(theta)*(t-t_0); ...
	       2*cos(thi)*sin(theta)      2*cos(thi)*cos(theta)*(t-t_0)   -2*sin(thi)*sin(theta)*(t-t_0); ...
	       2*sin(thi)-(t-t_0) 	  0				   2*cos(thi)*(t-t_0)]);

t2  =      [1/2 + 2*cos(thi)*cos(theta)*(t-t_0) - v*t; ...
	       2*cos(thi)*sin(theta)*(t-t_0) - 1; ...
	       2*sin(thi)*(t-t_0) - (1/2)*(t-t_0)*(t-t_0) - h];

result = t1*t2;
