% twopoint.m - compute a path between two poses
% 
% RMM, 1 May 07
%

function alpha = twopoint(pose1, pose2, time)
global nBasisFunctions;

for index = 1:nBasisFunctions
  % Initial position in x (Northing)
  M(1, index) = steering_basis(index-1, 0, 0.0);
  M(1, index+nBasisFunctions) = 0;
  zbar(1) = 0;

  % Initial position in y (Easting) 
  M(2, index) = 0;
  M(2, index+nBasisFunctions) = steering_basis(index-1, 0, 0.0); 
  zbar(2) = 0;

  % Initial angle and velocity 
  M(3, index) = steering_basis(index-1, 1, 0.0);
  M(3, index+nBasisFunctions) = 0;
  zbar(3) = cos(pose1(3));

  M(4, index) = 0;
  M(4, index+nBasisFunctions) = steering_basis(index-1, 1, 0.0);
  zbar(4) = sin(pose1(3));

  % Initial steering angle (zero curvature)
  M(5, index) = steering_basis(index-1, 2, 0.0);
  M(5, index+nBasisFunctions) = 0;
  zbar(5) = 0;

  M(6, index) = 0;
  M(6, index+nBasisFunctions) = steering_basis(index-1, 2, 0.0);
  zbar(6) = 0;

  % Final position in x (Northing) 
  M(7, index) = steering_basis(index-1, 0, 1.0);
  M(7, index+nBasisFunctions) = 0;
  zbar(7) = pose2(1) - pose1(1);

  % Final position in y (Easting) 
  M(8, index) = 0;
  M(8, index+nBasisFunctions) = steering_basis(index-1, 0, 1.0); 
  zbar(8) = pose2(2) - pose1(2);

  % Final point and velocity 
  M(9, index) = steering_basis(index-1, 1, 1.0);
  M(9, index+nBasisFunctions) = 0;
  zbar(9) = cos(pose2(3));

  M(10, index) = 0;
  M(10, index+nBasisFunctions) = steering_basis(index-1, 1, 1.0);
  zbar(10) = sin(pose2(3));

  % Initial steering angle (zero curvature)
  M(11, index) = steering_basis(index-1, 2, 1.0);
  M(11, index+nBasisFunctions) = 0;
  zbar(11) = 0;

  M(12, index) = 0;
  M(12, index+nBasisFunctions) = steering_basis(index-1, 2, 1.0);
  zbar(12) = 0;
end

% Compute the coefficients for the basis functions
alpha = M \ zbar';

